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ABSTRACT 


CoPiSiderations of real-time control problems for an Autonomous Underwater 
Vehicle (AUV) are addressed in this research. Among these problems is the ability to 
control the submersible given its highly nonlinear operating environment. In order to 
account for these variations, robust control techniques must be used. In particular, 
Variable Structure Control (VSC) with Doyle-Stein Observer has proven to produce 
optimal results while maintaining a high degree of robusmess. This led to the 
development of a real-time error detector using the robust observer to provide system 
redundancy through software. The culmination of this work is a real-time autopilot written 
in the "C" language which is ready for implementation and testing in the Naval 
Postgraduate School AUV prototype. 

We also address the aspect of real-time signal processing and conditioning in terms 
of Synchro-to-Resolver Conversion and anti-aliasing filters. The synchro problem involves 
converting a nonpotentiometric directional gyro output to a natural binary format which 
calls for an intricate design of power transformers, analog-to-digital converter, and passive 
element components. Lastly, the use of Generalized-Immittance Converter circuitry in the 
design of very low frequency anti-aliasing filter applications is developed and tested. 
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I. INTRODUCTION 


The Autonomous Underwater Vehicle (AUV) project was conceived at the Naval 
Postgraduate School in the early 1980’s. The purpose of the project was the development 
of an underwater vehicle as a testbed for research. Most of the early work on this project 
was on model development and applications of the hydrodynamic equations essential in 
modeling vehicle motions accurately. More recently autonomous navigation, obstacle 
avoidance, path planning, and advanced control techniques have become the focus of 
current research, in particular adaptive and nonlinear control design. 

To get an understanding of the goals of the project and the need for research in the 
aforementioned areas, a basic knowledge of the concept and scope of operation of the 
AUV is needed. The NPS vehicle is designed to be an untethered submersible capable 
of autonomous operation for an extended period of time. Conceptually, the vehicle is 
placed in an unknown environment where it will map the immediate surrounding area 
while evading static and dynamic obstacles. Upon completion of its mission, the vehicle 
will return to its base for retrieval and subsequent down-loading of its mission data. 

The submersible must be capable of operating in various dynamic environments in 
order to complete this type of mission. This can be achieved by adding a level of 
intelligence, at the expense of increased complexity of the real-time control problem. 

The purpose of this work is to design and implement a real-time control system for 
rapid maneuvers of the AUV. The technique used is based on Variable Structure Control, 


1 



with subsequent computer code developed in a high level language. Controller 
requirements are in terms of performance (i.e., fast response) and robusmess in the 
presence of modeling errors and sensor failures. 

A robust observer has been designed for the purpose of redimdancy so that system 
stability can still be maintained when one or more sensors (pitch and pitch rate, for 
example) fail. Upon successful modeling and testing of this complete autopilot, the code 
was converted into the "C" language for real-time implementation. The converted "C" 
code then was tested using an interface module that emulates real-time sampling 
conditions and acts as a link between model, controller, and local path planner. A further 
aspect of this work is the design and testing of various hardware components of the 
electronic signal processing and conditioning network proposed for the AUV. 

This thesis discusses issues ranging from automatic control systems to hardware 
implementation of electronic signal processing equipment. In Chapter II the autopilot 
control principle and algorithm are developed. Chapter III addresses considerations of 
real-time code development and testing. In Chapter IV hardware design issues concerning 
signal conditioning and information encoding are covered. The final chapter contains 
conclusions and general project assessment. All pertinent computer code and hardware 
designs are located in the appendices. 
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n. REAL-TIME CONTROL METHODS FOR DEPTH AND STEERING 

A. BACKGROUND INFORMATION 

An accurate model of the system must exist in order to develop and test control 
algorithms. This model should allow for all possible dynamic situations. In the case of 
the AUV, a model based on the United States Navy’s Swimmer Delivery Vehicle is used. 
This model is discussed at length in Chapter ID. The estimated dynamics of the prototype 
AUV helped to determine the control methodology used in the real-time algorithms. The 
uncertainties in the dynamic response of the vehicle due to the operating enviroment 
means a robust control scheme has to be used. The control techniques used in steering 
and depth are distinctly different due to these vehicle dynamics and purposed operating 
conditions. This chapter discusses the development, testing, simulation, and future real¬ 
time implementation of an autopilot for the AUV. The derivation in this chapter of the 
variable structure algorithm is based on work by Cristi [Ref. 1]. 

B. VARIABLE STRUCTURE CONTROL ALGORITHM DERIVATION 

This technique is used in the design of regulators for nonlinear time varying 
uncertain systems [Ref. 2]. Variable structure control, commonly referred to as sliding 
mode control, can be based on a nominal model, and it can account for uncertainties in 
the dynamic response of the plant. The main feature of this method of control is the fact 
that it is very robust in nature. Since the uncertainties can be handled in the control law 
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by assuming a known bound on the uncertainty. In particular let us consider a nonlinear 
state space model of the form 


x=f(x,u^) (2-1) 

where / is not exactly known. The function can be divided into the sum of a known 
portion and an uncertain component 

/ (x,u,0 = /o ix,u,t) + (x,u,t) (2-2) 


where /g is the known nominal function. A bound on the uncertainty a/ is assumed to be 
known as 

F ix,u,t) i I 4 (x,M,f) I (2-3) 


where F is a known for every x, u, and t. Although F could be arbitrarily large, an 
extreme value of the bound will result in imdesirable chattering of the control signal. T 
variable structure control technique is based on the definition of a sliding surface, <j(x) 
= 0, and switching law for which the feedback control is of the form 


I u^ix) if o(x) 2: 0 
I ufx) if o{x) < 0 


(2-4) 
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The switching law is determined so that the state is driven from any initial condition jc(to) 
to the sliding surface a(x) = 0 where it stays (ideally, at least) for all subsequent times. 
In selecting the sliding surface, the only condition which must be satisfied is that of stable 
dynamic response to any set of initial conditions which may be imposed. This can be 
expressed mathematically as the following: 

o(x(0) = 0 for all t > Xq — lim x(0 = 0 (2-5) 

Based on this, a possible choice of a(x) is a linear sliding surface 

o(x) = j (2-6) 

where 5 is a left eigenvector of A 

s^A = ( 2 - 7 ) 

This definition combined with the state space model 

X = Ax + bu + af (2-8) 

yields the dynamics of o. Thus, by combining (2-7) and (2-8) 

s^x = -ks^bu + s^iif (2-9) 
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this expression is obtained. Next let CT(t; = 5^x(t), a scalar signal, and substitute this 
expression into Equation (2-9) to obtain 

6(0 = -Xa(t) + s^bu ^ s^t>f ( 2 - 10 ) 


Now the control input u can be defined as 

u(0 = -Kit) sign{o{t)) (2-11) 

where K{i) is a time-varying gain to be chosen so that we define the Lyapunov function 

Via) = 1 iait)f (2-12) 

which guarantees that the sliding surface will reach 0, equilibrium, in a finite amount of 
time. This is depicted in Figure 1 which illustrates that, regardless of intial condition, the 
state trajectory is driven to the sliding surface. This can be found by multiplying Equation 
(2-10) by o(t) 

a(a(t)) = -kait)^ + s^buo + = Via) (2-13) 

which corresponds to the first derivative of the Lyapunov function. If we choose u as in 
(2-11)and 
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(2-14) 


m i 


5^F| 

's^b\ 


then 

V(o) <0 and V(o) - 0 (2-15) 

This in turn implies that a(x(t)) —> 0 (i.e., the state approaches the sliding surface a(x) = 
0). In fact, by Equations (2-10) and (2-12) we obtain 

V(a) = -Xo(0‘ - (5^h)^:(t)lo| + 5 Vo (2*^^^ 

In order for the sliding surface to behave as in Figure 1 over a finite interval of 
time, the following condition must be satisfied; 

d(f) = -fL(tfsign(o(t)) (2-17) 

The condition of Equation (2-17) is satisfied by choosing the control input as 

li + u = u = -(s^by's^Ax - (s ^b) 'sign( 0 ( 0 ) (2-18) 

Several important notes concerning the two previous equations should be addressed at this 
point. The selection of Ar(t) should be made only slightly larger than F, the known portion 
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Figure 1. Sliding Surface Representation 


of the uncertainty. This is due to the fact that, as the gain K increases, more chatteringwill 
be introduced on the sliding plane. From Equation (2-18), we note that the control u is 
composed of two parts, 

li = -{s^by^ s^Ax (2-19) 


which is a linear feedback law, and 

U = -K^(s^by'5ign{o) ^ 2 - 20 ) 
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This part is a nonlinear feedback with its sign toggling between plus and minus according 
to which side of the sliding plane the system is located in. The caveat placed on the 
magnitude of K must also include that it must be large enough to allow the system to 
possess the required degree of robusmess to handle the uncertainties brought about by the 
operating enviroment [Ref. 1]. 

C. COMPUTER CODING AND TESTING OF THE ALGORITHM 

The first programs generated were developed on the IBM-PC using Matlab for code 
generation. The real-time controller that was eventually realized was much more complex 
than this initial routine which only tested the nonadaptive sliding mode control derived 
in the preceding section. The Matlab code, dauv.m, is included in Appendix A. The 
primary puipose of this code was to test the basic depth controller and obtain a set of 
steady-state feedback gams. These gains were associated with the following states: pitch, 
pitch rate, and depth. The steady-state feedback gains were found using the linear- 
quadratic-regulator function in Matlab which is based on a solution to the Riccati 
equation. These gains were based on the nominal state space model of the SDV, and, as 
such, will require adjustment in the future to enable the AUV to perform as designed. 

The state space model used in the depth control algorithm is based on previous 
work by Schwartz [Ref. 3]. This model is discussed in detail in Chapter FV. The Matlab 
code is based on the variable structure development of the preceding se''tion. The steady 
state feedback gains are contained in the variable L which is a vector with three 
components. The program calls a model of the SDV which contains the dynamic 
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equations of motion developed by Boncal [Ref. 4]. In order to pass the updated 
information determined by this model, an array containing all twelve states has been set 
up. The states of this array which are of interest to the diving algorithm are: state(5) = 
pitch rate, state(9) = actual vehicle depth, and state(H) = pitch. In an effort to simplify 
the code and to reduce the error inherent with round off, the depth error, "z", is 
normalized with the absolute speed. The sliding surface is equated by multiplying the left 
eigenvector of the state space model with the error vector composed of pitch, pitch rate, 
and the normalized depth error. As shown in die derivation of the sliding mode controller, 
the input of the dive command is composed of a linear feedback component u and a 
nonlinear feedback portion u. The maximum deflection of the dive planes is 0.4 radians 
which is a software factor and has yet to be determined in hardware experimentation. 

1. Simulation Results 

The code is simulated using iteration techniques to varying orders of depth. In 
Figure 2 the graphical representation of the actual vehicle depth is shown. This graph 
shows that, for a commanded depth of 50 feet based on an initial or reference depth, the 
vehicle has no overshoot and settles to the desired depth change in a minimum of time. 
This simulation is based on a vehicle speed of approximately 3 knots which is the 
designed cruising speed of the AUV. Additional simulations have been run for varying 
depth commands with similar results (i.e., no overshoot with a minimum settling time). 
For the dive depicted in Figure 2, the corresponding dive plane action is shown in Figure 
3. We note that the action of the dive plane is smooth and does not exceed 0.4 radians. 
In direct correlation with the graph of the vehicle depth, we note that the dive plane 
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adjusts to not allow the vehicle to exceed a declination with respect to reference normal 
of approximately 0.8 radians. This is accomplished by feeding back the pitch rate and 
pitch states. Figure 4 is a depiction of the pitch rate which is a direct reflection of the 
divefm action. A measurement of the accuracy and smoothness of the vehicle trajectory 
can be assessed by graphical representation of the pitch rate. The pitch rate should possess 
a nearly symmetric shape with respect to either side of the mean value which is zero for 
constant trajectories. This is, in fact, the case of the graph in Figure 4. The last graph of 
this trial simulation represents vehicle pitch over the mn. When the submersible 
approaches the commanded depth, a gradual decrease in pitch should occur as depicted 
in Figure 5. This decrease in pitch should go to zero for constant trajectories as is the 
case. This indicates that the vehicle is coincident to the reference axis of the system. 

The last graph of this set. Figure 6, depicts the sliding surface behavior. This 
graph indicates that a stable sliding plane exist, and that, for the given initial condition, 
the state trajectory error will be driven to zero along this plane. Many simulations have 
been run for varying conditions and depths. All of the data collected indicated a stable 
and highly accurate controller which was the goal of this portion of the project. 
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Figure 2. Actual Depth for Variable Structure Controller Test 
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Figure 3. Divefin Response to a Change in Depth of 50 Feet 
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Figure 4. Pitch Rate Response to a Change in Depth of 50 Feet 



















Figure 5. Pitch Response to a Change in Depth of 50 Feet 
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Figure 6. Sliding Plane for Change in Depth of 50 Feet 








D. ROBUST OBSERVER DESIGN 

The variable structure algorithm developed in the previous section is very robust in 
behavior. The sliding mode control in general can handle any nonlinear perturbation due 
to this robustness. This concept is shown in Figure 7 where f, the nonlinear perturbation, 
which is possibly state dependent is shown. 


1 

(Unknown Noise Perturbation) 



X(t) 


U(t) / 

S_. 

Nominal 

y(t) 


State Space 
Model 

► 






Figure 7. Typical Sliding Mode Scenario 


When the state vector is replaced by its estimate, in general a degradation in the 
overall performance of the system occurs. This is due primarily to the fact that the 
estimator introduces errors into the loop. As a consequence, a controller which is 
satisfactory in a state feedback configuration might not perform in a desirable way once 
an observer is introduced. In this context we introduce the concept of robust observers in 
order to determine a class of observers for which the degradation of the closed loop 
system performance is minimal. 


17 








1. Robust Observer Derivation based on the Doyle-Stein Condition 

It is a standard result in system theory that in ideal situations the observer 
dynamics do not affect the dynamics of the closed-loop system. This can be seen easily 
by considering the equations of the closed-loop system 


X = Ax + Bu 
y = Cx 
u = -Lx + r 


^ = Ax * Bu * K(y - Cx) 

dt 


( 2 - 21 ) 


and deriving the transfer function (in the SISO case) Y(s)/R(s). However, this does not 
mean that the observer docs not have any impact in the dynamics of the overall system. 
In particular, we can see that in the case of a disturbance entering the input, for example, 
as 


X = Ax * biu + f) 
y - Cx 
u = -Lx + r 

^ = Ax * Bu * K(y-Cx) 

dt 


( 2 - 22 ) 


with / the disturbance term. The transfer function relating / to y is affected by the 
observer, and it could cause eithei instability of the system or poor characteristics in 
terms of phase and gain margins. 

However, when we know the precise point of entry of the disturbance / as in 
Equation (2-22), we can determine the observer gain K so to minimize its effect on the 
system performance. It has been shown in Ref. 10 that in the limiting case of K such that 
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(2-23) 


K[I + C(5/ - = BIC(5/ - 

the observer dynamics do not appear in the transfer function between / and y. As a 
consequence, the system (2-22) and its state feedback equivalent 

X = Ax + B(u + f) 

y = Cx (2-24) 

u = -Lx + r 


behave in the same manner. Condition (2-23) is called the Doyle-Stein condition and the 
resulting observer is referred to as a robust observer. 

It turns out that the Doyle-Stein condition (2-23) can be satisfied only as a limiting 
case. In other words it has been shown in Ref. 10 that a sequence of observer gains KiQ), 
with 

K{Q) = P«?)cX-' (2-25) 

where is an arbitary definite matrix and P{Q) is the solution of the Riccati equation 
APiQ) + + q^BB^ - P«?)c^P^-VP«?) = 0 (2-26) 

with q a scalar, positive parameter, is such that 
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(2-27) 


lim KiQ) = K 

with K satisfying the Doyle-Stein condition (2-23). 

The intuitive idea behind this agmment is to design an observer "as if' the 
disturbance / (not necessarily white or even random) were white noise with covariance 
q^. This leads to a steady-state Kalman filter design, Equations (2-25) and (2-26), where 
the degree of robustness of the observer increases as q increases [Ref. 5]. Therefore, we 
should be able to determine the optimal value of q using the Nyquist criterion and the 
continuous-time Nyquist plot. This plot should show a point of convergence for the 
maximum phase and gain margins capable of being achieved by our dive system. This 
point of convergence should give a value for q such that the dynamics of the observer do 
not effect the closed-loop characteristics of the system. Figure 8 is the continuous-time 
Nyquist plot for the dive system. We note that, as q increases in magnitude, the Nyquist 
plot converges to a optimal value. This value theoretically should give a optimum time 
response for depth and dive plane action. Figure 9 is a depiction of these time response 
plots for the value of q = 200 which was the point of convergence on the Nyquist plot. 
It is T vident from Figure 9 that the optimal value for the parameter "q” is not 200 as 
suggested by the continuous-time Nyquist plot and supported by the current 
documentation. 
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The dive plane response plot of Figure 9 suggested that q = 200 was too large 
in magnitude due to the chattering of the dive command. This led to a hueristic approach 
to determine the appropriate value for q. The optimal value for the dive system was found 
to be 5. The problem was how to justify this value mathematically and to prove the 
existence of a stability window that occurs when discrete controllers are used. 

a. Graphical Evidence of a Stability Window in Discrete-Time 

To study the behavior of the closed-loop system for different values of 
the parameter q, the discrete-time Nyquist plot was used. In discrete-time the Nyquist 
stability criterion is equivalent to the one in continuous-time, and the closed-loop behavior 
is inferred from phase and gain margins. Although we have shown in the previous section 
that the use of a robust observer improves the loop characteristics, in the actual 
implementation we have to exercise some care in the choice of the parameter q. This is 
due to the fact that we have assumed that the disturbance/in Equation (2-22) enters from 
the input only. If other sources of disturbances are present (i.e., entering at the output), 
they can introduce instabilities if the value of the parameter q in Equation (2-26) is too 
large. In order to verify this, let us break the loop at the input u and consider the transfer 
function Uj —> u, which is shown in the block diagram of Figure 10. This transfer function 
is derived in the following manner: 

X - Ax + Bu^ (2-28) 
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Figure 10. Block Diagram Showing System Broken at Input "X" and Output "XX" 
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u, = [Q L]x, , (2-36) 

the transfer function of —> u, in state-space form. We can compute the frequency 
response of the loop in Figure 10 open at the point marked with "X". Using this transfer 
function, a discrete Nyquist plot was obtained for various values of q: 5, 50, 150, and 
200. This plot is shown in Figure 11. From a comparison of this plot and that of Figure 
8, we see that the discrete time transformation effects caused no noticeable deviations in 
the frequency response for all values of the parameter q. 

As mentioned above, the only perturbation on the system taken into 
account so far is/which enters at the input as depicted in Figure 7, but in fact the system 
is also corrupted by noise at the output. If we want to see the effect of a disturbance at 
the output as q changes, we have to open the loop at the point marked "XX" in Figure 
10 and study this closed-loop transfer function ya yi- It can be shown that the state- 
space representation of the dynamics y 2 Yi is given by 

A -LB \K' 

" Q A-KC-Lb\ ^ [qJ (2-37) 

>’, = [C 0]x^ 
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The Nyquist plot of the corresponding frequency response depicted in 
Figure 12 shows some of the problems encountered for large values of q. We note that, 
while the phase margin and gain miirgin increase in Figure 11, a corresponding decrease 
is occurring in the phase margin of Figure 12 for an associated increase in the parameter 
q. Therefore, the noise present at the output causes a reverse effect on the system for 
increasing values of q. The effect of this is chattering in the dive plane response which 
occurs for large values of q. As the gain and phase margins increase at the input, the 
phase margin is diminishing at the output of the system. 

We can see from the Nyquist plots in Figure 12 that as q increases the 
gain margin decreases. In particular, from the phase margin, we can see that the closed- 
loop response of the system seen from a disturbance entering at the output becomes more 
and more under-damped for larger values of q. Experimental results show that a value of 
q = 5 yields a reasonable closed-loop response. This can be seen by comparing the step- 
response plots of the robust observer depicted in Figure 13 for the value of q = 5 with 
those of q = 200 in Figure 9. 

The continuous-time behavior of the Doyle-Stein condition appears to not 
account for the noise at the output in a similar fashion to that of the discrete-time. 
However, further investigation of this topic is warranted. Graphical evidence does support 
the adverse effect of a nonlinear perturbation present at the output in discrete-time control 
for submersible vehicles such as the Naval Postgraduate School Autonomous Underwater 
Vehicle. 
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E. SENSOR ERROR DETECTION AND HANDLING IN SOFTWARE 

The purpose of the robust observer for the AUV, which is depicted in block 
diagram form in Figure 14, is to provide redundancy in systems through software vice 
hardware. The observer developed in the previous section provides a certain degree of 
redundancy in the control of the AUV. In fact, for every signal in the loop (pitch, pitch 
rate, and depth in our case), both the measured and estimated signals are available. 

Based on this we can determine an error detection algorithm which alerts the system 
in case of sensor failure and replaces the faulty values by the estimates. This offers an 
advantage in terms of reliability, and it allows for a stable control even in the presence 
of one or more sensor failures. The principle of this error detection scheme is simple. 
Through the use of a fast computer algorithm, it checks error thresholds between 
measured state and observer estimated state. Then the algorithm selects the most accurate 
state representation and uses it for control. By this we mean that, if an error was detected 
in the sensor output, the estimated state would be used instead of the faulty measured 
state. 

1. Development of the Error Detection Algorithm 

The error between actual state, x. and estimated state, A, is defined as 

x^x - X (2-38) 
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Figure 14. Block Diagram of Robust Observer 
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The measured state is obtained in the AUV prototype directly from the sensors (i.e., 
gyros, sonars, and servo-controllers). Each sensor sends the information to a signal 
conditioning network and then to a designated area in memory where the autopilot 
samples the data. In order for the error detection concept to be realized, the typical error 
signal, X , should be as small as possible during normal operations, at least 
asymptotically; however, transient responses due to the observer dynamics introduce 
errors even in ideal conditions. A graphical representaion of the pitch rate and pitch error 
signals in Figure 15 shows that x in the transient phase is far from zero. These 
transients are present due to the initial conditions placed on the observer. In this section 
we determine an error criterion which is not affected by the transient response of the 
observer. Our arguments are an application of the Cayley-Hamilton theorem. 

In the ideal case the error between actual and estimated state satisfies the state 
space equation 


+ 1) = 4>(,z(k) 


(2-39) 


with ^>0 the discrete transition matrix of the observer, of the form 

<I>o = ® - A'C (2-40) 
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In our particular case, the state error x is given by 
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(2-41) 


where v is vehicle forward speed. Since the observer gain K is chosen to guarantee 
exponential stability of Equation (2-39), we can see that x(ife) has an effect only during 
the transient response. In order to determine an error criterion which is not affected by 
the transient response and is zero (or very small) during normal operations, consider the 
Cayley-Hamilton theorem applied to the matrix <I>o, 

= 0 (2-42) 


where 


.4(X) = A" - a, A"-' + ^ a„ = det(A/ - ^p) (2-43) 

is the characteristic polynomial of Application of Equation (2-42) to Equation (2-39) 
yields, at least under ideal conditions, 

e{k) = x{k) + a,x(A:-l) + +a^x{k-n) = 0 (2-44) 
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for all k, since 


xik) + a^x(Jc-\) + •• + ajik-n) = 

(©o" ^ aj)x{k-n) 

which is zero by the Cayley-Hamilton theorem. 

2. Testing of the Error Detection Algorithm 

The enor detection algorithm was tested by simulating a sensor failure. This 
was done in the code auvobs.m found in Appendix A. To simulate a sensor failure, the 
sensor in question is driven to zero to indicate a broken connection and to 100,000 to 
indicate the program accessing a null pointer region. Both of these error conditions reflect 
possible error sources which can actually occur. To notify the program developer and user 
that the system had detected an error, "C" language standard output prinrf statements were 
used. The actual real-time code documents error conditions differently by setting flags in 
a file that contains the vehicle run data. Also a faulty sensor or broken connection will 
probably not result in a polled value of 0.0, but for simulation purposes this value was 
used to show the sensitivity of the error detection algorithm. 

The algorithm takes the corrected error vector and compares the values with 
the threshold levels previously mentioned. If the corrected errors are within the tolerance 
setup by the threshold values, then the actual state is used, else the observed state is used 
and an error is assumed to have occurred. Several mns depicting various error conditions 
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are shown in Figures 17, 18, and 19. These figures only show the magnitudes present 
under these error conditions. In Figure 17 thcerror simulated is a broken connection to the 
pitch rate gyro. When the plots of Figure 17 are compared to those of Figure 16, we note 
the large magnitude of error present at the sensor failure point. This error diminishes 
rapidly, and, even though a sensor error is still present, the errors are below the threshold. 
Therefore, the autopilot does not need to discriminate between the observer state and the 
actual state. 

This error diminishes rapidily because the controller drives the error to zero 
in steady state and thus the effects of this type of error (i.e., a returned value of 0.0 from 
the failed sensor) are negligible. However, in Figure 18, a nuU pointer assignment error 
is simulated. This type of error occurs when a program, such as the sensor polling 
program for the AUV, accesses a region of memory which is marked for i.ull 
assignments. When this occurs, the value returned can take on a spurious assignment such 
as the one modeled in the simulation. From these plots we note that, regardless of the 
magnitude of the error, the other sensors are not effected. This is due to the fact that the 
sensors are independent of each other in hardware and software. The final set of plots 
shown in Figure 19 demonstrate a pitch sensor failure. This plot along with the others 
shows that, when a sensor failure occurs, the resultant error exceeds the threshold level 
and causes the observer state to be used. Once again the failure value of 0.0 was used to 
demonstrate the sensitivity of the error detection method. While the pitch sensor failed, 
we note again that the pitchrate sensor is not affected, and the error does not exceed the 
threshold level. 
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Threshold Levels for Sensor Failure Detection 






























Figure 18. Plots of Pitch and Pitch Rate with Threshold Levels for Sensor Failure Detection 















Figure 19. Plots of Pitch and Pitch Rate Errors with Threshold Levels for Sensor Failure Detection 














3. Fatal Sensor Errors 


Most sensors can be modeled with sufficient accuracy to be used in place of 
the actual state, but some errors, if detected, are considered major and cause mission 
termination upon detection. Three such errore are depth sensor error, directional gyro 
error, and erroneous input to the autopilot from the mission planner. All of these 
conditions are closely monitored, and if any one of these conditions is detected the AUV 
programmed mn is terminated. The AUV is forced to surface and slows to the minimum 
required speed necessary for the vehicle to remain on the surface. The rudder is also set 
to zero deflection when a fatal error occurs. 

Depth sensor gyro failures are detected in the same manner as those of the 
pitch and pitchrate gyros. A threshold level is compared to the corrected error returned 
by the difference of the estimated and actual states. This concept is depicted in block 
diagram forni in Figure 20. This diagram demonstrates how all of the components are 
linked in the algorithm. 

The error detection method used for sensing mission planner errors and 
directional gyro failure is different from the previous scheme. This algorithm does a check 
of the incoming values and compares these values to 2k. If the incoming value is > 2k. 
then a sensor error has occurred. The vehicle is forced to surface and the rudder is set to 
0 deflection. This terminates the AUV mission and protects the prototype vehicle from 
being damaged due to sensor failure or incorrect desired-headings generated by the 
mission planner. 
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F. SIMULATION OF REAL-TIME AUTOPILOT 

With the development of the robust observer and the error detection algorithm, the 
autopilot routine is complete and ready for simulation and implementation. The final 
check of the proposed autopilot was conducted simulating the "C" source code using the 
programs and interface module discussed in Chapter 3. The Matlab simulation code, 
auvobs.m, is located in Appendix A while the "C” source code, autopilot.c, is located in 
Appendix B. 

Daring this final evaluation period, a multiple dive simulation was run. This 
simulation detected a fault in the error detection algorithm. This fault occurred because 
the error vectors must go through two iterations of the control routine after a new ordered 
depth is received. This is due to the fact that the error vectors operate on previous states 
as shown in Equation (2-24). To correct this fault, a flag is used which sets the variable 
"transient_flag" to a value such that the error \ectors are allowed to update prior to being 
operated on by the coefficents of the characteristic equation of the A matrix. 

In the C code version of the autopilot, the current ordered depth is compared to the 
previous ordered depth. If the values are identical then error checking continues, but, 
when these values are different, a flag "transient_hold" is set using a discrete counter 
which increments each time the autopilot routine is called. This allows the error detection 
algorithm to operate in a discrete mode as intended. The transient flag can only be re¬ 
initialized when a change in ordered depth has been sent to the controller. 
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A multiple depth change simulation is depicted in Figures 21-24. This simulation 
was based on the following initial conditions and simulated errors: 

• Initial Depth: 100 Feet. 

• Initial Velocity: 4 Knots. 

• Initial Propeller Revolution Rate: 500 RPM. 

• Parameter q set to 5. 

• First Ordered Depth: 140 Feet. 

• Second Ordered Depth: 120 Feet (Begins at Frame 226). 

• Duration of Simulation Run: 400 Frames. 

• Simulated Error in Pitch Rate Gyro (Pitch Rate Sensor = 100000). 

• Simulated Failure of Pitch Gyro (Pitch Sensor = -35). 

• Pitch Rate Threshold Error set to 0.005. 

• Pitch Threshold Error set to 0.05. 

• Normalized Depth Error set to 0.2. 

The simulation has two induced sensor errors. The first error occurs at time-frame 5 when 
the value of 100000 is returned during polling of the pitch rate gyro. The second error 
occurs at time frame 50 when the value of -35 is returned during sampling of the pitch 
gyro. The pitch rate gyro error is an example of a null pointer assignment while the pitch 
gyro error is a general sensor failure. As noted before, these are both plausible values for 
their associated errors. In Figure 21 plots of the corrected pitch and pitch rate errors with 
their associated thresholds are shown. From these plots we note the beginning of the 
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sensor failure by the jump in the error. Both errors cause the direct sampled states to not 
be used and instead the estimated states returned by the observer are used. The noticeable 
drop in magnitude of these two errors that occurs at frame 226 is due to the change in 
ordered depth which calls for resetting the error vectors. The hiatus required to reset the 
error vectors causes the magnitude of both errors to momentarily fall, but, when the error 
detection algorithm is reset, the errors rise and again cause the observed state to be used 
vice the actual measured state in the controller. The time in seconds of this hiatus is 
approximately 0.1 seconds, which even for a slow sampling system such as the AUV, is 
negligible. The next set of plots shown in Figure 22 are of the normalized-depth error and 
actual pitch rate of the vehicle. The plot of normalized-depth error demonstrates that the 
errors induced in the pitch and pitch rate gyros have no effect on it. From previous 
simulations this is what we expect. The depth error does not exceed the threshold, and, 
therefore, a fatal sensor error is not detected. The plot of the pitch rate of the vehicle is 
a good indicator of the system stability. The vehicle pitch rate curve should be symmetric 
about the mean which is 0 in steady state. This indicates a stable platform and is, in fact, 
the type of curve shown in Figure 22. Figure 23 depicts plots of the actual depth with the 
ordered depth overlain and the stem plane or dive fin response. This set of plots shows 
that the controller, even with two sensor failures, is capable of driving the vehicle to the 
desired depth with no overshoot and a very smooth motion. The dive plane response is 
nearly optimal and also does not show any influence of the sensor errors on its control. 
This demonstrates the robust nature of the controller-observer combination. The last plot 
contained in Figure 24 is the final verification of system stability. This plot is of the 
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sliding plane. As mentioned earlier in this chapter and depicted in Figure 1, the controller 
should drive the state trajectories to the equilibrium point along this switching plane. This 
is the case shown Figure 24. From the sliding plane, we can deduce that two errors 
occuned during the mn. The change in ordered depth also caused the states to be offset 
at time-frame 226. The final result of the sliding plane was the states were in fact driven 
to equilibrium. This is the last necessary component needed to prove stability in the 
variable structure controller design. 
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Figure 24. Plot of the Sliding Plane 














in. CONSIDERATIONS FOR REAL-TIME-IMPLEMENTATION OF THE 

AUV CONTROLLER 

A. DEVELOPMENT OF A PC-BASED AUV MODEL 

«• • .. . 

To facilitate development and testing of the various programs to be implemented 
on the prototype-vehicle, a computer model of the vehicle dynamics has been developed. 
This model presents an interface that is similar in nature to the actual hardware 
implementation. By this we mean that the sampling of the actual measured states on the 
prototype by the control programs should be similar to the methods used in simulation. 
A model for the purposes of simulation was previously developed by Schwartz [Ref. 3] 
which described the dynamics of a 17.4 feet long vehicle, weighing 12000 pounds, and 
having neutral buoyancy. This previous work was taken as the basis of the real-time 
model developed here. 

The model has six degrees of freedom: three referring to position, the Euler angles, 
and three to rotation. Figure 25 shows the right-hand coordinate system of the vehicle 
which the control code is based on, as well as an artist’s rendition of the AUV. To define 
the coordinate system setup by these angles, the vector of orientation and rate of change 
had to be taken in to account. In order to describe the right-hand coordinate system for 
computer computations, the model consists of the 12 states listed in Table 1, where the 
mathematical symbol, state definition, and computer program variable are annotated. 

In Appendix C the program, model.c, and header file, mo ". Iprm.h, which contains 
the parameters of the AUV have been listed. This version of the model features the states 
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in array format which allows the calling routines to access the model by passing a pointer 
to the first address of the array. This is a significant improvement over the previous 
version of the model which used all gobal variables, a method that has been generally 
shunned by programmers. Due to the capabilities of the "C" language, the visibility of 
functions and variables can be easily modified [Ref. 6]. (A variable with global visibility 
can be accessed by any function and modified, where as a variable which is locally 
visible to its parent function only cannot be modified by any other function.) 

The actual AUV implementation will use these visiblity rules in a similar manner 
to that of the integrated test package which uses the model to update the states for 
simulation purposes. This updated model also corrected several serious coding hazards 
such as the initialization of an array beginning with an index of 1 vice 0, which led to an 
extraneous use of zeros for array padding. This critical mistake can cause null pointer 
assignments which result in run-time errors if a program references the array outside of 
its bounds. Another notable improvement which lends the model more to real-time- 
simulation code development was the update method used for the control surfaces, rudder, 
stemplane, bowplane, and propeller revolution rate. These values are passed in a similar 
manner to the states (i.e., using pointers). 

The model calls one external function, signum.c, which accepts a variable that has 
been declared as a double, and returns a double which has the value of -i-l.O or 
-1.0 depending on the sign of the passed argument. The function modeU) passes the 
following arguments: a pointer to the address of the first value of oldstate, a pointer to 
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the address of the first value of inputs, and the sampling interval. It returns the address 
of the first value of the updated state array and a integer which can been used to 
determine if an error has occurred during the state update. 


Table 1. STATE DEFINITIONS FOR AUV MODEL 


Mathematical 

Symbol 

State 

Definition 

Program 

Variable 

x-y-z-axis 

Definition 

u 

surge rate 

mstate[0] 

x-axis 

velocity 

V 

sway rate 

mstate[l] 


w 

heave rate 

mstate[2] 

z-axis 

velocity 

P 

roll rate 

mstate[3] 

rotation rate 
about x-axis 

q 

pitch rate 

mstate[4] 

rotation rate 
about y-axis 

r 

yaw rate 

mstate[5] 

rotation rate 
about z-axis 

X 

surge 

mstate[6] 

x-axis 

y 

sway 

mstate[7] 

y-axis 

z 

heave 

mstate[8] 

z-axis 

<}> 

roll 

mstate[9] 

rotation 
about x-axis 

e 

pitch 

mstate[10] 

rotation 
about y-axis 

¥ 

yaw 

mstate[ll] 

r tation 
about z-axis 
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Figure 25. Sketch of the AUV with Euler Angles "From Ref. 5. 















B. INTEGRATED SIMULATION ENVIRONMENT INTERFACE PROGRAM 
The control code, developed in chapter 2, and the model simulation code required 

an interface program that would mock the actual hardware integration with the software. 
The program, auvs.c, performs this vital interface function, as well as demonstrating how 
field evaluation methods and troubleshooting with AUV run data can be accomplished. 
It features a scheme for calling the functions which make up the real-time software and 
the simulation code. It also gives one method for storing vehicle nm data. This was the 
method used in the testing and simulation of the autopilot program which used the Doyle- 
Stein Observer. The suggested method for the actual AUV run data storage would be to 
open a data file upon completion of the autopilot update. This would allow the current 
position data and control surface actions to be congruent; however, this is not the order 
found in the interface program because the system, like all simulations, has a set of initial 
conditions which perturb the control system to perform a set of actions. This was the only 
simulation difficulty which could not be accounted for in the integrated package. 

C. FIELD EVALUATION GRAPHICS ROUTINE 

The data processed by the AUV during an actual deployment may have the need 
for immediate graphical interpretation. This case could arise due to a malfunctioning 
program or sensor. It was therefore essential to write a graphics routine which would take 
the uninterpreted data and graphically depict it with little effort by the evaluator. 
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1. C Functions used to Implement the Graphics Routine 

The main program, plot.c, calls numerous functions to perform various tasks. 
The majority of these functions are system calls which are specific to Microsoft C version 
5.1. Therefore, it is imperative that any future modifications made to the files composing 
the graphics routine be compiled using the Microsoft C 5.1 compiler. To invoke the 
executable file, type graph <fllename> . 

The data file should be set up in the same format as that produced by the 
interface program. In particular, the file should consist of column headings followed by 
the output of a particular state, sensor, or control surface in column format. The first 
column of the data file will always be taken as the abscissa. Therefore, to ensure that the 
graphs represent meaningful data, time should always be the first column of the data file. 
The ordinate label is the current heading of the data file. The source code for all of the 
graphics files are in Appendix D. 

a. Main Program PLOT 

This file sets up the graphics environment, as well as actually creating the 
graphs. Numerous system functions once again are used; the prototypes and descriptions 
of these functions can be found in Microsoft C version 5.1 documentation. The main 
program calls one user-defined function which is graphics_mode. The next section deals 
with this function in detail. A header file is also associated with the graphics files, grph.h. 
This file contains structures and prototypes common to both of the primary files. 
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The program begins by setting up buffers for the storage of data file 
headings, data, graph titles, and graph subtitles. The defaults for plot representation are 
white foreground, black background, and solid line representing the plotted data. The user 
is prompted for the use of grids on the graph. The only correct replies to this prompt are 
lowercase y or n; any other response will cause the prompt to reappiear. Ehie to the nature 
of the program, which is to plot dynamic arrays, the program must perform some learning 
tasks. Therefore, it is not as rapid a plotting routine as one that knows the size of the 
array prior to compilation time, but the capability to plot varying array sizes is a 
requirement for field evaluation. The program determines the array size by reading the 
data file the first time through. E>uring this first read, the program determines how many 
columns to expect and the length of the columns or number of rows in the array. The 
program does not store or act on any of the actual data until the second read. 

The capability of the program to dynamically set the data storage area is 
a feature which is a strong point of C [Ref. 6]. The plots are displayed in succession (i.e., 
order is determined by the column in the data file), and are held on the screen until the 
user presses the return key. The graphs are autoscaled, no manual scaling is available to 
the user, so that the largest and smallest values are depicted. A typical output of the 
program is represented by Figure 26. Prior to terminating, the executable code resets the 
video mode of the computer system back to its previous mode. 
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b. Function GRAPHICS_MODE 

This function is contained in the file, setvid.c, and is called by the 
function, plot.c. It does not pass any arguments and returns a void. The function checks 
the computer system for a graphics card. If a graphics card is present, the card is 
automatically placed in its highest resolution mode. This will cause a problem if the 
system that the program is running on has a Video Graphics Adapter card (VGA) which 
is set for enhanced mode, and the system is using an enhanced graphics adapter monitor. 
The program will automatically set the graphics adapter card to its highest setting which 
in this case is VGA; this results in the monitor receiving incorrect graphic information 
and will cause screen disruption or system lockup. Therefore, the graphics program should 
not be used in situations like this. The function tests for all common graphics cards. If 
no graphics card is present, the program informs the user that a graphics card is necessary 
and then terminates. 
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Figure 26. Typical Output of Graphics Program 














rv. ASSOCIATED AUV HARDWARE 


The hardware development and designs presented in this thesis are by no means all 
inclusive of the AUV project. The AUV project has been distributed hardware 
responsibilities among many individuals. The common bond has been the GRIDCASE 
1535 EXP laptop computer and the associated data translation board. The initial designs 
of the AUV electronics called for an intricate network of sensors and control servos. 
These devices would be sampled and sent control signals, respectively, by the Grid 
interface network. This network consists of signal conditioning modules, power supplies, 
and sensors. A portion of the network is depicted in Figure 27. 

The Grid computer was to initially contain all of the code for control, local path¬ 
planning, and mission-commander. This basic premise has grown to the current design 
which calls for a two computers, the Grid and a Gespack. The Gespack computer is a 
rack-based system which runs the OS9 operating system. It contains a separate data 
translation board from that of the Grid and is designed to operate both autonomously or 
networked. In this scheme the Grid acts as a quasi-Lisp-machine, since it mns a Lisp 
interpreter while not being a true Lisp-machine. It contains the Lisp code written for the 
global path-planner while the Gespack runs the control code for AUV mission. This 
modified hardware scheme still requires an elaborate interface network for signal 
conditioning. The following sections deal with two different aspects of the signal 
conditioning problem, filtering and synchro conversion of the rate gyro signals. 
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A. LOWPASS ACTIVE ANALOG FILTER 


This lowpass active filter was designed to be used as an anti-aliasing filter. Its 
relative position in the signal interface network is shown in Figure 27. In order to 
determine the filter cutoff frequency the open-loop bandwidth of the dive control 
system had to be determined along with the sampling frequency. The sampling frequency 
for the AUV prototype is proposed to be 20 Hz. This results in a significant oversampling 
ratio which is discussed in more detail later in this section. In order to determine the 
open-loop bandwidth of the dive control system, the transfer function or state space model 
for that system had to be derived. The open-loop Bode plot could then be plotted and the 
bandwidth determined. 

The discrete-time transfer function for the dive system was developed by Schwartz 
[Ref. 3J. The corresponding continuous-time dynamics were derived and estimated by 
Davis [Ref. 7], and its transfer function has been estimated to be 


H{s) = 


0.04 

sis^ + Q.ls + 0.04) 


(4-1) 


which corresponds to 
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in state-space format where q, 0, and z are defined in Table 1, and the input u is the dive 
command signal. This transfer function is based on a linearized model operating at 300 
RPM. With the state space model defined, an open-loop Bode plot, magnitude only, was 
then produced. The code, auvbode.m, used to produce this plot is located in Appendix A. 
The Bode plot is depicted in Figure 28; it represents the dive system dynamics and gives 
us a numerical value for the system bandwidth. The Bode diagram demonstrates the low 
frequency nature of the AUV dive system. Because the system bandwidth is 
approximately 0.2 Hz and the sampling frequency is 20 Hz, the system is being 
significantly oversampled. This simplifies many associated sampling problems and insures 
that the Nyquist criterion 

/, > 2 /^ ( 4 - 3 ) 

is satisfied [Ref. 8]. In determination of the cutoff frequency needed for the anti¬ 
aliasing filter, the bandwidth was rounded up to 0.5 Hz. This fact, coupled with the 20 
Hz sampling frequency /„ meant that the lowpass filterwas required to be within the 
range of 5 Hz to 15 Hz. This is due to the fact that the next harmonic of the system will 
occur at a frequency equivalent to the sampling frequency /, of 20 Hz. Based on these 
facts, the lowpass was set at 10 Hz. Also due to the high sampling frequency with 
respect to the system bandwidth, a reconstruction filter associated with the anti-aliasing 
filter was not needed [Ref 8]. 
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Figure 28. Open Lxx)p Bode of the Diveplane System 















































1. Numerical Development of Active Lowpass Filter 


The active-rcsistor-capacitor filter, active-RC, development is based on the 
generalized-immittance converter (GIC) circuit. This is due to the fact that active-RC 
filters can be derived directly from passive-RLC prototype networks by replacing the 
passive inductors with active, GlC-simulated inductors as shown by Ghausi and Laker in 
Ref. 9. The development of the basic transfer function for the active-RC filter using a 
biquadratic GIC scheme is also shown in Ref. 9. From this basic derivation, the generic 
lowpass filter transfer function can be derived. This is the basis of the analytical and 
numerical development of the lowpass active-RC filter designed. TTie determination of the 
appropriate values for the circuit components was based on 



(4-4) 


where is the cutoff frequency and 

w, = 2nxl0® (4-5) 

is the gain bandwidth product. In addition to these parameters, the following design 
specifications were considered; maximally flat Butterworth filter (i.e., Q = 0.707), 

= 10 Hz, and negligible phase shift in the frequency region of interest. 
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To limit the amount of noise introduced by this circuit, the resistor values were 
limited to 100 kfl. Therefore; solving Equation (4-4) for the capacitor, a value of 0.159 
pF was foimd. The complete schematic circuit diagram is located in Appendix E. Due to 
the fast sampling rate, a second-order Butterworth filter with a slow roll-off was designed. 
This circuit is a monatomic design; that is, for each sampled sensor a lowpass anti¬ 
aliasing filter exists as shown in Figure 27. 

2. Evaluation of Numerical Results 

Based on the results of the previous section, a numerical transfer function was 
detennined. This transfer function was based on the generic lowpass filter function 
derived in Ref. 3. Using this transfer function the Bode diagrams representing the 
magnitude and phase of the designed lowpass filter were obtained and are depicted in 
Figures 4-2 and 4-3, respectively. The magnitude graph shows that, for an ideal op amp, 
the circuit’s 3-dB frequency is 10 Hz, as called for in the specifications. While the phase 
plot shows that in the frequency region of interest (i.e., 0.2 Hz), a negligible phase shift 
exists, as desired. 

The experimental results were well within a tolerable deviation from the ideal 
results. These results were also supported by simulation using Spice, a computer circuit 
simulation program. Due to the very low frequency involved, a GlC-circuit was a logical 
choice. This is due to the sensitivity of the circuit which has been proven in related 
experimentation [Ref. 9]. 
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B. SYNCHRO-TO-RESOLVER CONVERSION 

In order to determine the direction or Heading of the AUV, a highly accurate Flux 
Gate Gyro was purchased. The gyro specifications are 400 Hz, 26 volt reference, with an 
11.8 volt line signal. Due to these specifications which required higher voltages and 
subsequently produce higher output voltages than are capable of being handled by the 
system hardware, a conversion scheme had to be worked out so that the line voltages 
were within a specified range (i.e., 2.0 volts), but still maintained the needed accuracy 
which is inherent in the 11.8 volt line signal. To overcome this problem, a set of chips 
was purchased which included the following: reference isolation transformer, signal 
isolation transformer, and analog-to-digital converter. The problem that had to be resolved 
was the design of this circuit including all of the passive elements which are used to set 
various parameters on the transformer chips. 

1. Directional Gyro Background 

Directional gyros have Hvo degrees of freedom. The first degree of freedom 
referred to as the inner gimbal axis, allows the spin axis to be maintained horizontal with 
respective to the fixed Earth reference coordinate system. The second degree of freedom, 
referred to as the azimuth or outer gimbal axis, detects and indicates movements of the 
vehicle in azimuth from a heading reference. This reference in the case of the AUV is 
along the x-axis or the surge-axis as previously defined. Therefore, it is apparent that the 
spin axis of the directional gyro must continue to point in a fixed direction in a horizontal 
plane. The term fixed direction refers to a mode, slaved , or type of gyro which makes 
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corrections in order to maintain a fixed orientation with respect to the Earth reference 
frame. In order to do this, most gyros require the corrections to be made externally; 
hence, the role of the flux gate is revealed. A slaved gyro is precessed in azimuth by an 
external reference signal such as a magnetic transmitter. In this case the magnetic 
transmitter (flux gate) continuously aligns the azimuth gimbal to magnetic north. 
Therefore, the overall accuracy of the gyro depends in a large part on the flux gate 
output. We note that spurious magnetic anomalies and interference will have an adverse 
affect on the flux gate output and thus result in inaccurate headings. Due to this sensitive 
characteristic, the flux gate on the ALTV has been encased in a nonmagnetic metal alloy 
known as mu-metal. TTiis is intended to lessen the probability of error induced by 
magnetic interference. 

Equipment associated with gyros are typically power supplies, and synchros, 
or resolvers. Usually, these pieces of equipment are internal to the overall housing of the 
gyro. The power supply performs an obvious task, but some discussion the function 
of the synchro and resolver is warranted. The synchro and resolver perform identical 
missions, that of encoding the gyro output on a signal to enable other equipment to have 
access to this information. The primary difference in the two schemes is the method of 
encoding the direction information. 

The rotor and stators in a resolver are oriented 90 degrees with respect to each 
other as in Figure 31. This electrical arrangement results in the following 
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resolver shaft angle equations; 

R = iisin (tot) (4-6) 

5,_3 = y4sm (tot) sin (0) (4-7) 

5^ 2 = /Isin (tot) cos (6) (4-8) 

where R represents the rotor excitation voltage which is the ac reference voltage, and 0 
is the resolver shaft angle. Thus the output voltage which apjjears across stator terminals 
Sj-S, and S^-Sj contains the necessary angle information. This information must *>e 
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converted into a usable fomiat (i.e., natural binary or dc), hence the need for resolver 
conversion. [Ref. 10] 

The synchro rotor-stator configuration differs from the above resolver format 
in that the orientation of the stators with respect to each other is 120 degrees. This is 
depicted in Figure 32. Due to this orientation of the stators, three phases of shaft angle 
information exist while in resolver format only two phases exist as previously shown. The 
resulting synchro format voltage equations are: 

Sj 3 = i4sm (wf) sin (0) (4-9) 

~ ^sin (cot) sin (0 + 120°) (4-10) 

52.1 “ 240°) (4-11) 

These equations represent the line signal voltage which carries the shaft angle 
information. The rotor excitation voltage is the same as that of Equation (4-6); also the 
amplitude of the line voltage signal is the same as the reference signal. This is logical 
because the ac-reference signal acts as a carrier for the line signal [Ref. 10]. The problem 
that had to be solved was how to interpret the information contained on each line signal 
coming from the stators. The three-phase output produced by the synchro had to be 
converted to a usable format for interface with the computer. 
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Figure 32. Simple Synchro (After Ref. 7.) 


2. Synchro Conversion Circuit Design 

The output format of the flux-gate gyro presented several problems to the 
system designers of the AUV. Some of these problems included the high line signal and 
reference voltages. The data translation board would only accept a maximum of +5 volts 
rms. The desired voltage was +2 volts rms. Another consideration was how to handle the 
information presented in a 3-phase output such as the one present in the flux-gate gyro, 
and lastly what type of information format was desired. The data translation board can 
accept both analog and digital inputs. This section deals with one solution to the problem. 
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a. Component Selection 

Presented with the format problem mentioned above and the compatible 
output required to interface with the data translation board, the following components 
were selected: 5S70/411, 5S72/26V, 2S80KD, and AD767KN. These components are all 
manufactured by Analog Devices tc perform the task of synchro-to-resolver conversion. 
The additional passive components required in the design are off-the-shelf resistors and 
capacitors. 

The 5S72/26V is a reference-signal-isolation transformer. It takes the 
reference signal used in the rotor excitation. Equation (4-6), as a input. The transformer 
then steps down this voltage to the standard 2 volts rms used by most analog-to-digital 
converters. The transformer also provides isolation for the electronics from the resolver. 

The 5S70/411 is a line-signal-isolation transformer. It accepts the output 
of the synchro stators. Equations (4-9), (4-10), and (4-11), as a input. The 5S70/411 
converts the three-phase 11.8 volt signals into a two-phase 2 volt nms resolver format. 
This is the format required by most analog-to-digital converters used in control and sensor 
applications. The 5S70/411 is designed specifically for synchro-to-resolver conversion of 
11.8 volt line signals. 

The 2S80KD is a monolithic tracking resolver-to-digital converter. This 
converter uses a ratiometric tracking conversion technique which provides continuous 
position data without conversion delay. The ratiometric conversion technique is based on 
the ratio of the resolver format voltages being equal to the tangent of the shaft angle. 
Therefore, the tracking converter is not dependent on the absolute magnitudes of the 





signal input [Ref. 10]. It also allows the circuit designer to select passive components to 
define the appropriate bandwidth and the desired resolution of the converter. 


The AD767KN is a digital-to-analog converter. It accepts a 12-bit binary 
input and converts it to a analog output. This particular component was not used in the 
synchro-to-resolver conversion design, but can be implemented without the need for 
handshaking in the circuit design which appears in Appendix E. TTiis chip would be 
needed only in the event that the binary input lines to the data translation board are 
required for additional purposes other than the directional gyro information. In this case 
the 12-bit binary output of the 2S80KD would need to be converted to a analog signal 
and input into the data translation board in this manner. 

b. Design Considerations 

The passive component selection was based on the desire to achieve the 
highest possible bandwidth and resolution. The components selected were all within 5% 
tolerance. In reference to the circuit schematic found in Appendix E, the resistors R1 and 
R2 alongwith the capacitors Cl and C2 makeup an HF filter. The purpose of this filter 
is to reduce the amount of noise present on the signal inputs to the 2S80. The values of 
Cl and C2 are dependent on R1 which is equal to R2. If the value of R1 is selected based 
on optimal value given by the manufacturer, the following equation can be solved for the 
values of Cl and C2: 


C7 = C2 = -=- 

2nRl 


(4-12) 
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The resistor R4 is a gain scaling resistor whose value is determined by 


R4 = -— (_L) fi 

100 X lO '* 3 


(4-13) 


For resolution of 16 bits, the DC-error component is 0.0025. Components R3 and C3 act 
as a all-pass filter. TTiese components introduce a phase shift in order to offset any phase 
shift introduced by the circuit at the reference frequency. The values for these components 


are R3 ■= lOOikQ and C3 > - . The maximum tracking rate is set by the 

voltage-controUed-oscillator input resistor R6. This is accomplished by selecting the 
desired maximum tracking rate T„,„ which is not to exceed 1/16 of the reference 
frequency. 


R6 


5.92 X 10^ 


(4-14) 


The maximum tracking rate is the highest angular speed for which the converter output 
is able to keep track with the input. The tracking rate has a direct relationship to the 
reference frequency (i.e., the nigher the reference frequency, the higher the tracking rate) 
[Ref. 11]. 

The closed-loop bandwidth of the converter is selected by adjusting 
capacitors, C4 and C5, and resistor R5. The bandwidth of the converter is a measure of 
the acceptable variance of a sinusoidal input to the converter (i.e., a 80 Hz bandwidth 
converter will accept a varying input sinusoid < 80 Hz) [Ref. 11]. This implied that the 
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converter for the AUV did not require a large bandwidth because the directional gyro 
line-signal will vary slowly. While this is, in fact, the case, the circuit was designed with 
a 80 Hz bandwidth to allow for any future modifications to the associated systems of the 
AUV. The trade-off for the increased bandwidth was an increase in the value of R5 by 
20% while C4 and C5 decreased in value 30% and 50% respectively. The following 
equations were used in determining the appropriate values of the bandwidth components: 

/jtEF ^ (4-15) 


20.2 X IQ-^ 

R6 X 


C5 = 5 X C4 


R5 = -1- Q 

2ll/^jyC5 


(4-16) 


(4-17) 


(4-18) 


Offset and bias current at the integrator input of the 2S80KD can cause 
an additional error. This error can be lessened or completely eliminated with the use of 
an offset bias circuit. This offset bias circuit is realized in the design by placing a resistor, 
R8, at the integrator input and tying its output to a potentiometer, R9, which is coupled 
across the ± 12 volt dc source . The values of these components, as well as C6 and R7 
are given in Reference 12.[Ref. 12] 
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c. Evaluating the 2S80KD Converter Output 

Many possible methods exist for representing angular information in 
digital form. The most common method is natural binary coding. The most significant bit 
in this system represents 180 degrees, the next 90 degrees, and so on. A representation 
of bit and angle equivalence is listed in Table 2. This table shows the accuracy possible 
with the converter set in 16 bit resolution mode. In this mode the accuracy is 
approximately 19.8 arc seconds. The circuit can be modified to represent the output in 
binary coded decimal (BCD) for tests and evaluation by sending the output of the 
2S80KD to a binary to BCD converter. The resulting output can then be sent directly to 
a seven-segment decoder for visual display purposes. 






Table 2. NATURAL BBVARY BIT CONVERSION SCHEME 


Bit Number 

Angle in Degrees 

1 MSB 

180.0 

2 

90.0 

3 

45.0 

4 

22.5 

5 

11.25 

6 

5.625 

7 

2.8125 

8 

1.40625 

9 

0.70313 

10 

0.35156 

11 

0.17578 

12 

0.08790 

13 

0.04395 

14 

0.02197 

15 

0.01099 

16 

0.00549 
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V. CONCLUSIONS 


The Variable Structure Controller has been shown to be robust and to work under 
adverse operating conditions. This makes it well-suited for application in the AUV. 
Simulation results have shown that, despite external disturbances, the system’s response 
converges to a desired equilibrium. In order to preserve this property, a robust observer 
was designed to estimate the signals in the loop. This observer design is based on the 
Doyle-Stein robust observer condition. 

The observer signals are used in an error detection algorithm, which detects the 
presence of a sensor failure. The purpose of this is to eliminate the need for redundant 
hardware systems, while stdl maintaining reliable control operation in case of sensor 
failure. The simulation results show that the autopilot equipped with the sensor failure 
detector we developed can still maintain stability in the presence of sensor errors. This 
is conceptually again a new approach to an old problem that of system redundancy. 
When dealing with Autonomous Underwater Vehicles, new methods for system 
redundancy are required due to space limitations. This autopilot algorithm has proven to 
be accurate and as sensitive to deviation in the sensor errors as the user desires. The 
inclusion of the error detection algorithm did not result in any degradation in efficiency 
of the overall autopilot. 

When converting the autopilot algorithm to the "C" language, an interface module 
was necessary to link the converted algorithm to a converted model. The model used to 


81 







test the real-time autopilot had to be modified to simulate the envisaged sensor sampling 
of the actual hardware. This modified model behaved identically to the original "C" 
model which declared all variables as global. 


The anti-aliasing filter design was tested in a lab environment with very satisfactory 
results. The very low frequency of this filter and its intended use called for a special 
design. The GIC active-filter was proven very well suited for the application. 

The software developed for the autopilot is ready to be implemented on the onboard 
computer of the AUV (Gespack). Wet-bed testing of the whole system in various 
maneuvers is the next step in the development of the autopilot. 
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APPENDIX A. HIGH LEVEL DEVELOPMENT SOURCE CODE 
This appendix contains the high level source code used in testing and evaluation of 
the autopilot algorithm. The source code is in Matlab . a "C"-based development 
language. TTiis source code was used to produce all graphical representations presented 
in this thesis. It is not part of the real-time code intended for the AUV prototype which 
is written exclusively in "C", and is located in Appendix B. A copy of this source code 
can be obtauicd from Professor Roberto Cristi, Naval Postgraduate School (see Initial 
Distribution List for mailing address). 

Matlab is a registered trademark of The Math Works. Incorporated . It does not 
produce stand-alone executable code, but instead is an interactive program intended for 
numerical problem solving. This program is not available with the source code in this 
appendix. Matlab is required when using the source code listed in this appendix. 
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% Filename "auvobs.m" 

% Doyle • Stein observer for continuous model of AUV 
% This program was used to develop and evaluate the real-time 
% autopilot code for the AUV. It calls a model that is written in 
% "C" language to update the states using the dynamic equations 

% motion for the submersible, 
clear 

ERXP=[’Failure simulated in pitch at 50 ( Sensor Pitch = -35 )’]; 
ERXQ=[’Error induced in pitch rate at 5 ( Sensor Pitch Rate = 100000)’]; 
ERXQP=[’Sensor Pitch Rate = 100000 at 5, Sensor Pitch = -35 at 50’]; 
q=input('Input your q for this mn ’); 
a=.04 ; 
b=.7 ; 

A = [ -b -a 0 
1 0 0 

0 -1 0]; %!!!!!! notice the minus signs 

% s=[l; 0.2070;-0.0198] % left real eigenvector of A-B*L 
s=[l; 0.7; -0.04]; % left eigenvector of A for lambda=0 

B = [ -a;0;0]; % to get model states to track right 

C =[00 1]; 

D =0; 

Qe = [ .01 0 0 

0 .01 0 

0 0 . 01 ]; 

Re = [ 1 ]; 

G = 1; 

qc = [ 1 0 0 

0 1 0 

0 0 1 ]; 

rc = [10]; 
dt=.25; 

phi=eye(3)-i-A*dt; 

del=B*dt; 

L=lqr(A.B,qc.rc) % returns optimal feedback gains 
Q = Qe*l + q^2*B*B’; 
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Kd=lqe(A,G,C,Q,Re); % returns optimal kalman gains 
Kd=Kd*dt 

% observer dynamics; 
phiO=phi-Kd*C; 
ac=poly(phiO) 

kmax=400; 
ordered_depth= 150; 
previous=ordered_depth; 
depth=100; 
rpm=500; 
xdot=4.0; 
transient_flag=3; 
thresl=.001; 
thres2=.01; 
thres3=.0l; 

abspeed = xdot; 
pitchrate=0.0; 
pitch=0.0; 
r=[zeros( 1 Jcmax) 
zeros( 14 cmax) 

depth*ones(l,30) ordered_depth*ones(l,kmax-30)]; 
state = zeros(12Janax); 
xo = zeros(3,kmax); 
uk = zeros(l,kmax); 
error = zeros(3,kmax); 
auv_normalz = zeros(14imax); 
initial_state = [xdot;0;0;0;pitchrate;0;0;0;depth;0;pitch;0]; 
xo(;,l) = [pitchrate;pitch;-depth/abspeed]; % initialize observer 
state(;,l) = initial_state; 

.-.main loop- 

for j=l:kmax-l 
if j > 500, 

ordered_depth=l 20; 
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end; 

%-Allow for Error Matrix to Change States- % 

if previous ~= ordered_depth, 
transient_flag = j+2; 
end 

u=L(3) *ordered_depth/abspeed; 

u=u-L*[state(5,j);state(l 1 ,j);state(9,j)/abspeed]; 

z=state(9,j)-ordered_depth; 

q=state(5,j); 

th=state(ll,j); 

% check the error to the threshold 
if abs(error(l,j)) > thresl, 
p_rate=xo(lJ); 

^rintf(’pitchrate is using obs\n’) 
else 

p_rate=q; 

end 

if abs(error(2,j)) > thres2, 
pit = xo(2,j); 

fprintf(’pitch is using obs\n’) 
else 

pit = th; 
end 

if abs(error(3,j)) > thres3, 
norm_dep_err = xo(3J); 
fprintf(’normalized depth error is using obsNn’) 
else 

norm_dep_err = z/abspeed; 
end 

ex=[p_rate; pit; norm_dep_err]; % setup to compute nonlin 

ste(j)=s”''ex; 

ubar=1.0*(ste(j)); 

if (abs(ubar) > 0.4) 
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ubar=0.4*sign(ste(j)); 

end 

u=u+ubar; 
divfin = u; 
if (abs(divfin) > .4 ) 

divfin = 0.4*sign(divfin); 
end; 

inputs=[0; divfin;-divfm;rpm]; 
u = divfin; 

state(: ,j+1 )=model(state(: ,j),inputs,dt); 


uk(j+l)=u; 

xo(:,j+l)= phi*xo(:,j) + del*u ; 

xo(;,j+l)= xo(:,j+l) + Kd*(z/abspeed - C%xo(;,j))); 

abspeed=sqrt((staie(l J+l ))^2+(state(2,j+l ))^2+(state(3,j+l ))^2); 

% — simulate a fault in q (pitchrate) 
if j>5; 

q=100000; 

end 

% — end fault 

% — simulate a fault in theta (pitch) 
if j>50; 

th=-35.0; 

end 

% — end fault 

% transient correction for the observer 

xe(:, j+l)=[q;th;z/abspeed]-xo(:,j); 
if j>transient_flag, 

error(:,j+l)=ac(l)*xe(:,j+l)+ac(2)’''xe{;,j)+ac(3)*xe(:.j-l)+ac(4)*xe(:,j-2); 

end 

pre vious=ordered_depth; 

end % end of main loop 
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% Begin plotting routine 


subplot(211),plot(xe(2,.)), title(’Uncorrected Pitch Error’) 

subplot(212),plot(xe(l,:)), title(’Uncorrected Pitchrate Error’) 

%nieta uncorE 

pause 

clg 

axis([0 kmax -.2 .5]) 
subplot(211),plot([0 kmax],[.05 .05],’:’) 

%text(300,-. 14, ’Threshold ’); 
tc?:t(300,.05,’Threshoid'K 
hold on; 

plot{abs(error(2,:))), title(’Corrected Pitch Error’) 

%text(.13,.525,ERXP,’sc’); 

hold off; 

axis([0 kmax -100 1000]) 
subplot(212),plot([0 kmax],[.005 .005],’:’) 
text(300,.005,’Threshold’); 
hold on; 

plot(abs(error(l,:))), title(’Corrected Pitchrate Error’) 

text(.09,.015,ERXQ,’sc’); 

hold off; 

meta errbigq 

pause 

clg 

axis([0 kmax -.2 .4]); 

subplot(211),plot(abs(error(3,:))), title(’Depth Corrected Error’) 
hold on; 

plot([0 kmax],[.2 .2],’:’) 
text(300,.2,’Threshold’); 
hold off; 

subplot(212),plot(state(5,:)), title(’Pitch Rate’) 
text(.l,.03,ERXQP,’sc’); 
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%meta simderr 
pause 

clg 

axis([0 kmax 100 160]); 

subplot(211),plot(state(9,:)),title(’ Model Depth’) 
hold on; 

plot(t0 225 226 kmax],[140 140 120 120],’:’) 
text(20,140,’Ordered Depth’); 
hold off; 

axis([0 kmax -.45 .45]); 
subplot(212),plot(uk),title(’Divefin Action’) 
text(.l,.03,ERXQP,’sc’); 
pause 

%meta simdd 
clg 

subplot(21 l),plot(state{ll,:)),title(’Pitch ’) 
subplot(212),plot(xo(2,;)),title(’ Observer Pitch ’) 
pause 
clg 

subplot; 

axis; 

plot(ste),title(’ Sliding Surface’),grid 
text(.l,.03,ERXQP,’sc’); 


t 


I 
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% NON-AOAPnVE VARIABLE STRUCTURE DIVEFIN CONTROL 
% filename = dauv.m 

% This program is used to determine the optimal feedback gains of 
% the fiillstate feedback sliding mode contoUer. It also test the 
% accuracy of the variable structure controller. 

[ear; 
a=.04 ; 
b=.7 ; 

A = [ -b -a 0 % State space model of Dive system 

1 0 0 

0 -1 0]; %!!!!!! notice the minus signs 
s=[l; 0.2070;-0.0198]; % left real eigenvector of A-B*L 
B = [ -a;0;0]; % to get model states to track right 

C =[00 1]; 
qc = [ 1 0 0 

0 1 0 

0 0 1], % error covariance noise 

rc = [10]; % measurement noise 

dt=.25; 

L=lqr(A,B,qc,rc); % Optimal Feedback gains 

kmax=400; 
depth=0; 
rpm=600; 
xdot=4.0; 

am=A-B*L ; 

[eigvec,eigval]=eig(am’); 
lambda=real(eigval( 1,1)); 

s=eigvec(:,l) ; % Left eigenvector of dive system 

st=s’ 

abspeed = xdot; 

pitchrate=0.0; 

pitch=0.0; 

state = zeros( 124 tmax); 
uk = zeros(l,kmax); 







auv_normalz = zeros(ljkmax); 

initial_state = [xdot;0;0;0;pitchrate;0;0;0;depth;0;pitch;0]; 
state(:,l) = initial_state; 

%-main loop- 

i = 0; 

for j=l:kmax-l, 
if (J < 180). 

ordered_depth=50; 

else, 

ordered_depth=70; 

end 

u 1 =L(3 )*ordered_depth/abspeed; 
u=u 1 -(L*[state(5 ,j );state( 11 ,j );state(9,j)/abspeed]); 
z=state(9,j )-ordered_depth; 
q=state(5,j); 
th=state(ll,j); 
ex2=[q;th;z/abspeed]; 
ste(j)=s’*ex2; 
ubar=1.0*(ste(j)); 
if (abs(ubar) > 0.4) 
ubar=0.4*sign(ste(j)); 
end 

u=u+ubar; 
divfm= u; 

if (abs(divfin) > .4 ) % Limit maximum deflection to .4 radians 

divfm= 0.4*sign(divfin); 

end; 

inputs=[0;divfin;0;rpm]; 
u= divfin; 

state(:,j+l)=model(state(;,j),inputs,dt); % Call the dynamic equations model 
uk(j+l)=u; % Save u for plotting purposes 

abspeed=rpm/i 50; 

% abspeed=sqrt((state(l,j+l))''2+(state(2J+l))^2+(state(3,j+l))^2); 
time(j) = dt * i; 
timel(j) = dt * i; 
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i=i+l; 

end 

time(j+l) = dt * i; 

% Begin plotting routine 
axis([0,70,-l,51]); 

plot(time,state(9,:)),xlabel(’Tinie (Sec)’),ylabel(’ Depth (Feet)’),grid 

%meta chap2_l 

pause 

axis([0,80,-.2,.4]); 

%plot(tinie,uk),xlabel(’Time (Sec)’),ylabel(’ Divfin Action’),grid 
plot(time,uk),title(’AUV Run #1 Dives of 50 ft and 70 ft’), 
xlabel(’Time’),ylabeI(’ Divfin ’),grid 
meta chap3_2 
pause 

axis([0,70,-.08,.l]); 

plot(time,state(5,;)),xlabel(’Time (Sec)’),ylabel(’Pitch Rate’),grid 

%meta chap2_3 

pause 

axis([0,70,-.8..1]); 

plot(time,state(ll,:)),xlabel(’Time (Sec)’),yIabel(’Pitch’),grid 

%meta chap2_4 

pause 

axis([0,70,-.05,.25]); 

plot(timel,ste). xlabel(’Time (Sec)’),ylabel(’ Sliding Surface’),grid 
%meta chap2_5 
axis; 
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% Dive system BODE Plot 
% filename = auvbode.m 

% This program determines the Bode plot of interest for 
% the AUV dive system. 

clear; 

% Set initial model conditions 
qll = 1 ; 

q22 = 1 ; 

q33 = 1 ; 

r = [0;0;0]; 
dt= 0.25 ; 

kmax = 400 ; 


al= [ -.7 -.04 0 
1 0 0 
0 -1 0 ]; 
bl = [ -.04 
0 
0 ]; 

q=[qll 0 0 
0 q22 0 
0 0 q33]; 

c=[0 0 1]; 

[num.den] = ss2tf(al,bl,c,0,l) 

w=logspace(-3,3,250); 

[mag,phase]=bode(al ,bl ,q,r, 1 ,w); 
axis([-3,2,-150,50]); 

semilogx(w/(2*pi),20*logl0(mag)),title(’Magnitude Response for AUV Dive Controller’) 
xlabelCFrequency (in Hertz)’),ylabel(’Magnitude (in dB)’) 
text(.002,-25,’Pitchrate’) 
text(.002,2,’Pitch’) 
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text(.01,25,’Dive’) 
grid 

% Continuous-Time Nyquist Plot 
% filename q_cnyq.m 

% This program is used to determine the effect of varying q in the program 
% auvobs.m. This program performs the nyquist plot for a family of q’s. 

clear; 

cig; 

axis([-l.l 1.1 -1.1 1.1]); 

axis(’square’); 

dt=.25; 

% Plant definitions 
A=[-.7 -.04 0 
1 0 0 
0 -1 0 ]; 

B=[-.04; 0; 0]; 

C=[0 0 1]; 

s=[l; 0.7;-0.04]; % left eigenvector of A for lambda=0 
D =[0]; 

Qe = [ .01 0 0 

0 .01 0 

0 0 . 01 ]; 

Re = [ 1 ]; 

G = 1; 

qc = [ 1 0 0 

0 1 0 
0 0 1 ]; 
rc = [10]; 
dt=.25; 

L=lqr(A,B,qc,rc); % returns optimal feedback gains 

UR = zeros(3); % upper right block of ’Ae’ (A extended matrix) 

Be = [ B 
0 
0 
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01; % B extended 

Ce ■= [0 0 0 L]; % C extended 

w=logspace(-1,1,150); 

temf>q=input(’Input 4 values of q as a vector (ie. [ ]) ’); 

% Begin loop for family of curves 

plot(exp(sqrt(-l)*[0:0.05:7]),’-’); 
hold on; 

title(['Continuous Time Nyquist Plot’]),grid; 
for i = 1;4, 
q=tempq(i); 
if i==l, 

QTXl = [’— q=’,num2str(q)]; 
elseif i==2, 

QTX2 = [’... q='.num2str(q)]; 
elseif i==3, 

QTX3 = q='.num2str(q)]; 

else. 

QTX4 = q=’,num2str(q)]; 

end 

Q = Qe*l + q^2*B*B’; 

Kd=lqe(A.G,C.Q.Re); % returns optimal kalman gains 
LL = Kd*C; % lower left block of Ae 

LR = A-LL-B*L; 

Ae = [ A UR 

LL LRJ; %■ A extended 

[re.imj = nyquist(Ae.Be,Ce.D,l,w); 

if i==l. 

plot(re,im,’-’) 
elseif i==2. 

plot(re,im.';') 
elseif i==3, 
plot(re.im,’-.') 
else. 
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plot(re,im,’-’) 

end 

end 

hold off; 

xlabel(’Real’),ylabel(’Imaginary’); text(.05,.35,QTXl,’sc’); 
text(.05,.25,QTX2,’sc’);text(.05,.15,QTX3,’sc’);text(.05,.05,QTX4,’sc’); 

% Discrete Nyquist Plot Program for AUV Dive System 
% filename = q_nyq.m 

% l ;iis program is used to determine the effect of varying q in the program 
% auvobs.m. This program performs the discrete Nyquist plot for a family of q’s. 

clear; 

clg; 

axis([-l.l 1.1 -1.1 1.1]); 

axis(’square’); 

dt=.25; 

% Plant definitions 
A=[-.7 -.04 0 
1 0 0 

0 -1 0 ]; 

B=[-.04; 0; 0]; 

C=[0 0 1]; 

s=[l; 0.7; -0.04]; % left eigenvector of A for lambda=0 

D =[0]; 

Qe = [ .01 0 0 

0 .01 0 

0 0 . 01 ]; 

Re = [ 1 ]; 

G = 1; 

qc = [ 1 0 0 

0 1 0 

0 0 1 ]; 

rc = [10]; 
dt=.25; 

L=lqr(A,B.qc,rc); % returns optimal feedback gains 
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% upper right block of ’Ae’ (A extended matrix) 


UR = zeros(3); 

Be = [ B 
0 
0 

0]; % B extended 

Ce = [0 0 0 L]; % C extended 

tempq=input(’Input 4 values of q as a vector (ie. [ ]) ’); 

% Begin loop for family of curves 

plot(exp(sqrt(-l)*[0:0.05:7]),’-’); 
hold on; 

plot([-l 1],[0 0].’-’); 
for i = 1:4, 
q=tempq(i); 
if i==l, 

QTXl = [’— q=’jium2str(q)]; 
elseif i==2, 

QTX2 = [’... q=’,num2str(q)]; 
elseif i==3, 

QTX3 = q=’,num2str(q)]; 
else, 

QTX4 = [’ _ q=’,num2str(q)]; 
end 

Q = Qe*l + q''2*B*B’; 

Kd=lqe(A.G,C,Q,Re); % returns optimal kalman gains 
LL = Kd*C; % lower left block of Ae 

LR = A-LL-B*L; 

Ae = [ A UR 

LL LR]; % A extended 

% Begin discrete portion of program (ie. discretize Ae and Be) 

[PHIe,GAMe] = c2d(Ae,Be,dt); 
wr = logspace(-1.57,pi,250); 
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[mag,phase] = (ibode(PHIe,GAMe,Ce,D,l,wr); 
for k=l:length(mag), 

REAL(k)=mag(k)*cos(phase(k)*pi/l 80); 
IMAG(k)=mag(k)*sm(phase(k)*pi/l 80); 
end 

if i==l, 

plot(REAL,IMAG,’-’) 
elseif i=2, 

plot(REAL,IMAG,’:’) 
elseif i=3, 

plot(REAL,IMAG,’-.’) 

else, 

plot(REAL,IMAG,’-’) 

end 

end 

%title(’Discrete Nyquist Plot’) 
xlabei( ’Real ’ ),ylabel( ’Imaginary ’); 
grid; 

text(.05,.35,QTXl,’sc’); 

text(.05,.25,QTX2,’sc’); 

text(.05,.15,QTX3,’sc’); 

text(.05,.05,QTX4,’sc’); 

hold off; 

axis(’normal’); 

axis; 
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% State Feedback Steering Algorithm 
% filename = auvmd.m 

% AUV steering algorithim based on the feedback of current direction 
% and desired heading, 
clear 

%K = input(’Input gain ’); 
desired_head=input(’Input desired heading ’); 

K=.3; 

kmax=300; 

depth=0.0; 

head=0; 

rpm=500; 

xdot=4.0; 

dt = .25; 

abspeed = xdot; 

pitchrate=0.0; 

pitch=0.0; 

yawrate=0.0; 

yaw=0.0; 

state = zeros(123cmax); 
ruk = zeros(l Jkmax); 

initial_state=[xdot;0;0;0;pitchrate;yawrate;0;head;depth;0;pitch;yaw]; 

% states are 1234 5 6 78 9 10 11 12 

state(:.l) = initial_state; 

%.main loop- 

turn = desired_head-state(12,l); 
if turn > pi, 

fprintf(’Tum is more than 180 degrees’) 
desired_head = desired_head-(2*pi); 
end 

for j=l:kmax 

rud = -K*(desired_head-state(12,j)); 
rudder = rud; 
if (abs(rudder) > .4 ) 
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rudder = 0.4*sign(rudder); 
end; 

divfin = 0; 

inputs=[rudder; divfin;-divfm;rpni]; 
rud = rudder; 

state(: ,j +1 )=model(state(: j),inputs,dt); 
nik(j+l)=rud; 

abspeed=sqrt((state(lj+l))^2+(state(2,j+l))^2+(state(3,j+l)y'2); 

desired_head; 

state(12,j); 

end % end of main loop 

clg 

time=0:kmax; 

subplot(2H),plot(time,state(12,:)),title(’ Model Head’) 
subplot(212),plot(time,ruk),titie(’ Rudder Action’) 
text(.01,.03,’AUVRUD program with gain=.3’,’sc’) 

%meta pres 
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% Steering and Dive Simulation Program for the AUV 
% filename = auv.m 

% This program is the final phase test of the autopilot algorithm. 

% It links the VSC, Doyle - Stein Observer, state feedback steering, and 
% error detector components of the controller for full simulation. 

clear 

ERX=[’Error induced in Pitch Rate at time intervals of 15 and 130 (p = 0.0)’]; 
desired_head=input(’Input desired heading ’); 
ordered_depth=input(’Input ordered depth ’); 
q = 5 ; 
a=.04 ; 
b=.7 ; 

A = [ -b -a 0 
1 0 0 

0 -1 0]; %!!!!!! notice the minus signs 

s=[l; 0.7; -0.04]; % left eigenvector of A for lambda=0 
B = [ -a;0;0]; % to get model states to track right 

C =[00 1]; 

D =0; 

Qe = [ .01 0 0 

0 .01 0 

0 0 . 01 ]; 

Re = [ 1 ]; 

G = 1; 

qc = [ 1 0 0 

0 1 0 

0 0 1 ]; 

rc = [10]; 
dt=.25; 

phi=eye(3>fA*dt 

del=B*dt 

L=lqr(A,B,qc,rc); % returns optimal feedback gains 
Q = Qe*l q'^2*B*B’; 
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Kd=lqe(A,G,C,Q,Re); % returns optimal kalman gains 
Kd=Kd*dt; 

% observer dynamics: 
phiO=phi-Kd*C; 
ac=poly(phiO); 

lanax=400; 

depth=100: 

ipm=500; 

xdot=4.0; 

thresl=.001; 

thres2=.01; 

thres3=.01; 

K=.3; 

abspeed = xdot; 
pitchrate=0.0; 
pitch=0.0; 
yawrate=0.0; 
yaw=0.0; 
i^[zeros( 1 Jonax) 
zeros(13anax) 

depth*ones(l,30) ordered_depth*ones(13aTiax-30)]; 
state = zeros( 124 cmax); 
xo = zeros(34anax); 
uk = zeros(l,kmax); 
ruk = zeros(13cmax); 
error = zeros(3Jkmax); 
auv_normalz = zeros(13miax); 

initial_state=[xdot;0;0;0;pitchrate;yawrate;0;0;depth;0;pitch;yaw]; 
% states are 1234 5 6 789 10 11 12 

xo(:,l) = [pitchrate;pitch;depth/abspeed]; % initialize observer 
state(;,l) = initial_state; 
turn = desired_head-state(12,l); 
if turn > pi, 

fprintf(’Tum is more than 180 degrees'^’) 
desired_head = desired_head-(2*pi); 
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end 


main loop 


for j=l:kmax-l 


u=L(3 )*ordered_depth/abspeed; 
u=u-L*[state(5,j);state(l 1 ,j);state(9J)/abspeed]; 
z=state(9,j)-ordered_depth; 
q=state(5,j); 
th=state(ll,j); 

% check the error to the threshold 
if abs(error(lJ)) > thresl, 
p_rate=xo(l,j); 

fprintfCpitchrate is using obsNn’) 
else 

p_rate=q; 

end 

if abs(erTor(2,j)) > thres2, 
pit = xo(2,j); 

fprintf(’pitch is using obsVi’) 
else 

pit = th; 
end 

if abs(error(3,j)) > thres3, 
norm_dep_err = xo(3,j); 
fprintf(’normalized depth error is using obsNn’) 
else 

norm_dep_eiT = z/abspeed; 
end 

ex=[p_rate; pit; norm_dep_err]; % setup to compute nonlin 

ste(j)=s’*ex; 

ubar=1.0*(ste(j)); 

if (abs(ubar) > 0.4) 
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ubar=0.4*sign(ste(j)); 

end 

u=u+ubar; 
divfin = u; 
if (abs(divfm) > .4 ) 

divfin = 0.4*sign(divfin); 
end; 

u = divfin; 

xo(;,j+l)= phi*xo(;J) + del*u ; 

xo(:,j+l)= xo(;,j+l) + Kd*(z/abspeed - C*(xo(:j))); 

% simulate a fault in q 
if j>15 & j<130, 
q=0.0; 
end 

% end fault 

% transient correction for the observer 

xe(:, j+1 )=[q;th;z/abspeed]-xo(:,j); 
if j>3, 

error(;J+l)=ac(l)*xe(;,j+l)+ac(2)*xe(:,j)+ac(3)*xe(:,j-l)+ac(4)*xe(:,j-2); 

end 

% Simple AUV steering algorithim 

rud = -K*(desired_head-state(12,j)); 
rudder = rud; 
if (abs(rudder) > .4 ) 

rudder = 0.4*sign(rudder); 
end; 

inputs=[mdder; divfin;-divfin;rpm]; 
rud = rudder; 

state(: J+1 )=model(state(: ,j),inputs,dt); 

ruk(j+l)=rud; 

uk(j+l)=u; 
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abspeed=sqrt((state(l j+1 ))''2+(state(2,j+l ))^2+(state(3 J+1 )y'2); 
end % end of main loop 

% Begin plotting portion 
clg 

subplot(211),plot(error(2,:)), title(’Pitch Corrected Error’) 
subplot(212),plot(error(l,:)), title(’Pitch Rate Corrected Error’) 
teTt(.03,.04.ERX,’sc’); 

%meta pres 

pause 

clg 

subplot(211),plot(error(3,;)), title(’Depth Corrected Error’) 
subplot(212),plot(state(5,;)),title(’Pitch Rate’) 
text(.03,.04,ERX,’sc’); 

%meta pres 

pause 

clg 

subplot(211),plot(state(9,:)),title(’ Model Depth’) 
subplot(212),plot(uk),title(’ Divefm Action’) 
texf(.03,.04,ERX,’sc’); 

%meta pres 

% subplot(223),plot(state(5,:)),title(’Piich Rate’) 

% subplot(224),plot(xo(l,:)),title(’ Observer Pitch Rate ’) 
pause 
clg 

% subplot(221),plot(xo(3,:)),title(’ Observer Depth, etc’) 

% subplot(222),plot(ste),title(’ Sliding Surface’) 

% subplot(223),plot(state(ll,:)),title(’Pitch’) 

% subplot(224),plot(xo(2,:)),title(’ Observer Pitch ’) 
subplot; 

plot(ste),title(’ Sliding Surface’),grid 
text(.03,.04,ERX,’sc’); pause,clg 
subplot(211),plot(state(12,:)),title(’ Model Head’) 
subplot(212),plot(ruk),title(’ Rudder Action’) 
text(.01,.03,’AUVRUD program with gain=.3’,’sc’) 
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% filename = giclp.m 

% This file plots the magnitude portion of the gic bandpass active filter. 

% This was used in the determination of the required components of the 
% active-RC filter discussed in Chapter 4. 

% definitions 
clear; 

R=100e3; % resistor values 

G=l/R; 

wt=2*pi*le6; % gain bandwidth product 

%.start loop- 

QP=.707; % for maximally flat response 

rq=G/QP; 

C=.159e-6; % Capcitor value 

wc=l/(R*C); 

% -set up transfer function- 

numl=(2*C*G''2)Av'. 
num2=(2*G'^3+2*rq*G''2)/wt; 
num3=2*G'^3; 
num=[numl num2 num3]; 
denl=2*C^2*G/wt''2; 

den2=(2*G*C''2)/wt+(4*G^2*C)/wt^2+(2*G*C*rq)/wt^2; 

den3=C^2*G+2*G'^3/wt^2+2*G''2*rq/wt^2+4*G''2*C/wt+2*G*C*rq/wt; 

den4=2*G^3/wt+2*G^2*rq/wt+G*C*rq; 

den5=G''3; 

den=[denl den2 den3 den4 den5]; 

w=logspace(-l,4,250); 

[mag ,phase]=bode(num,den ,w); 

% Begin plotting routine 
axis([-l,2,-2,7]); 

semilogx(w/(2*pi),20*logl0(mag)),title(’Magnitude Response (LPF) Fc=10 Hz’) 
xlabel(’Frequency (in Hertz)’),ylabel(’Magnitude (in dB)’),grid 
text(.80,.03,’Q = .707’,’sc’); 
text(.05,.03,’Wt= 6.18e6’,’sc’) 
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pause 

axis([-1,3,-180.0]); 

semilogx(w/(2*pi),phase) 

title(’Phase Response (LPF) Fc=10 Hz’) 

xlabel(’Frequency (in Hertz)’),ylabel(’Phase’) 

grid 
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APPENDIX B. REAL-TIME CONTROL CODE 


This appendix contains the real-time autopilot code written in the "C" language. 
It has the following components: Variable-Structure Controller, Doyle-Stein Observer, 
error detector, and state feedback steering algorithms. This code is ready for 
implementation in the AUV. Also included in this appendix are two external functions 
called by the control routine, signum.c and matmul.c. 

Particular attention should be given to the comments preceding the code and to the 
disclaimer. This code was written and compiled in Microsoft C 5.1 . but is compiler 
independent (i.e., will compile on any ANSI standard C or K&R C compiler). A copy 
of this source code is available through Professor Roberto Cristi. Naval Postgraduate 
School (see Initial Distribution List). 
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/* 

AUV Autopilot Control Program Dave Riling / Prof. Cristi 
This program makes use of a Non-Adaptive Variable Structure Algorithim 
with an associated Doyle-Stein Observer used for error checking in the 
diving mechanism. For the steering a simply Single State Feedback control 
scheme is used. 

The following values were used in the determination of the (1) feedback 
gains, (2) observer gains, (3) eigenvalues and (4) transient response 
coefficents : 


A = [ -.7 -.04 0 

A matrix 

B = [ -.04 

B matrix 

1 0 

0 

0 


0 -1 

0] 

0] 


C = [ 0 0 1 

] C matrix 

D = [0] 

D matrix 


The associated matlab program which was used to evaluate the above 
values has the filename : " auvobs.m ". 

'fhe following values were obtained from the above program : 

(1) L = [ -3.6308 -2.8032 0.3162 ] 

(2) K = [ -0.0036 -0.0679 0.1859 ] 

(3) S = I 1.0 

0.7 

-0.04 ] 

(4) Ac = [1.0 -2.6391 2.3302 -0.6874 ] 

The threshold levels used in the sensor error detection algorithim were 

detennined using a hueristically graphic approach. 

threshold_l = .003 pitch rate threshold error 

threshold_2 = .05 pitch threshold error 

threshold_3 = .2 normalized depth threshold error 

The feedback gain used in the steering algorithim is " Ks = .175 ". This 
value was determined using a hueristic approach. 
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DISCLAIMER; Once the actual AUV model has been determined the matlab 
file " auvobs.m " should be changed and run to obtain the true values 
required for the actual AUV. Note this algorithim is designed to be 
a Real Time Implementation and is very Robust; as such, it should not 
require any changes other than the above parmeters if that. 

State Definitions 
mstate[0] = xdot 
mstate[l] = ydot 
mstate[2] = zdot 
mstate[3] = rollrate 
mstate[4] = pitchrate 
mstate[5] = yawrate 
mstate[6) = x 
mstate[7] = y 
mstate[8] = z 
mstate[9] = roll 
mstate[10] = pitch 
mstate[l 1] = yaw 

#include <math.h> 

#include <stdio.h> 

#include "auv.h" 


#define RUD_STOPS 0.4 
#define PI 3.141593 

#define TWO_PI 6.283186 
#define YES 1 

#define NO 0 


static double 

ex[] =(0.0.0.0.0.0 1. 


no 



L[] = I -3.6308, -2.8032, 0.3162 }, 

state[] = { 0.0, 0.0, 0.0 ); 


void control8( double *mstate, double *inputs, 

double ordered_depth, double desired_head, double ipm ) 

{ 

int order_change; 

jump_over = 0; 
double bowplane, 
stemplane, 
rudder; 
double q, 
theta, 
divres, 
obsres, 
abspeed, 
errz, 
divfinl, 
divfin, 

required_tum, 

Ks = 0.175; 
static double 

old_ordered_depth = 0.0; 

/* Now read speed sensor */ 

abspeed = mstate[0]; 

q = state[0] = mstate[4]; 

theta = state[l] = instate [10]; 

state[2] = instate [8]/abspeed; 

divfinl = L[2] * ordered_depth/abspeed; 

niat_inul(L, 1, 3, state, 3, 1, (Matrix)&divfm); 

divres = divfinl - divfin; 

if (ordered_depth != old_ordered_depth) { 

ordered_change = YES; /* set flag to indicate a change in ordered_depth) */ 

} 
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else { 

ordered_change = NO; 

} 

old_ordered_depth = ordercd_depth; 

errz = (mstate[8] - ordered_depth)/abspeed; 

ex[0] = instate [4]; 

ex[l] = mstate[10]; 

ex[2] = errz; 


/*— Call Diveplane Function —*/ 
diveplane(ex, &divres); 

if (observer(q, theta, errz, divrcs, order_change, &obsres)) { 
stemplane = obsres; 
bowplane = -stemplane; 

) 

else { 

stemplane = divres; 
bowplane = -divres; 


/*.start steering algorithim-*/ 

if (desired_head > TWO_PI) { 

printfC Error in desired heading coming from mission commanderNn"); 

printfC AUV surfacing and slowing to 150 RPM, rudder 0 deflection\n"); 

stemplane = RUD_STOPS; 

bowplane = -RUD_STOPS; 

rpm = 150; 

rudder = 0; 

jump_over = 1; 

) 

if (mstate[l’] > TAVO_Pl) { 

printfC’ Error in directional gyroNn"); 

printfC AUV surfacing and slowing to 150 RPM, rudder 0 deflectionVi"); 
stemplane = RUD_STOPS; 
bowplane = -RUD_STOPS; 
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rpm = 150; 
rudder = 0; 
jump_over = 1; 

} 

if (jump_over != 1 ) { 

required_tum = desired_head - mstate[ll]; /* Make the shortest turn */ 
if (required_tum > PI) { 

desired_head = desired_head - (2*PI); 

} 

rudder = -Ks * (desired_head - mstate[ll]); 
if (fabs(nidder) > RUD_STOPS) 

rudder = RUD_STOPS * sign(rudder); 

) 

/♦-movedata ????????????????-.V 

inputs [0] = rudder; 
inputs[ll = stemplane; 
inputs [2] = bowplane; 
inputs[3J = rpm; 


y ♦*****)»(***** 3*< 3((3*< j*e * ***** )#f :(c ***** **♦♦♦♦♦♦*%******♦*>*'*♦*****♦**♦***** ★ 

**** 

This function determines the observer states and also performs 
error & threshold comparisons to check sensor validity. The arguments 
are ; pitchrate, pitch, normalized error in depth, current divefin 
value, state of order_change, and observer divefin value 

*/ 

int observer!double q, double theta, double errz, 

double divres, int order_change, double *obsres) 

I 

#define No_Error 0 

#define Sensor_Error 1 

#define Major_Error 5 

int i, flag; 


113 







static int 


transient_hold = 3; 
j = 0; 


double 

PHID = 

{ 0.8250, -0.01, 0.0, 

0.2500, 1.00, 0.0, 

0.0, -0.25, 1.0 }; 

double 

DEL[] = { -0.01, 0.0, 0.0 }, 

CD = { 0.0, 0.0, 1.0 ), 

K[] = { -0.0036, -0.0679, 0.1859 }, 
EiTorD={0.0, 0.0, 0.0); 

static double 


obserr[] = { 0.0, 

0.0, 

0.0 }, 

obslerr[] = { 0.0, 

0.0, 

0.0 }, 

obs2err[] = { 0.0, 

0.0, 

0.0 }, 

obs3err[] = ( 0.0, 

0.0, 

0.0 ), 

old_obst[] = { 0.0, 

0.0, 

0.0 }, 

obstate[] = { 0.0, 

0.0, 

0.0 ); 


double cobs, 

comp, 

threshold_l = .003, /* pitch rate threshold error */ 

threshold_2 = .05, /* pitch threshold error */ 

threshold_3 = .2, /* normalized depth threshold error */ 

A1 = 1.0, /* transient error coefficents */ 

A2 = -2.6391, 

A3 = 2.3302, 

A4 = -0.6874; 


mat_mul(PHI, 3, 3, old_obst, 3, 1, obstate); 
DEL[0] *= divres; 

DEL[1] *= divres; 
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DEL[2] *= divres; 


mat_mul(C, 1, 3, old_obst, 3, 1, (Matrix)&cobs); 
comp = errz - cobs; 
for (i = 0; i < 3; i++) { 

obstate[i] += DEL[il; 

K[i] *= comp; 
obstate[i] += K[i]; 
old_obst[i] = obstate[i]; 


) 

/*—Correct for Transient Response—*/ 
obserr[0] = q - obstate[0]; 
obserr[l] = theta - obstate[l]; 
obserr[2] = errz - obstate[3]; 
if (order_change == YES) 
transient_hold=j+3; 
if (j > transient_hold) { 
for (i = 0; i < 3; i++) { 

Error[i] = Al*obseiT[i] + A2*obslerr[i] + A3*obs2err[i] + A4*obs3err[i]; 

) 


j++; 

for (i = 0; i < 3; i++) { 

obs3err[i] = obs2err[i]; 
obs2err[i] = obslerr[i]; 
obslerr[i] = obserT[i]; 


/♦—Compare the error to the Threshold values—*/ 
flag = 0; 

if (fabs(Error[0]) > threshold_l) { 

printfC Error[0] = %lf \n",EiTor[0]); 
ex[0] = obstate[0]; 
flag = Sensor_Error; 
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if (fabs(Error[l]) > threshold_2) { 

printfC Error[l] = %lf'«'',Error[l]); 
ex[l] = obstate[l]; 
flag = Sensor_Error; 

I 

if (fabs(Error[2]) > threshold_3) { 

/* Big Problem depth sensor is malfunctioning SURFACE SURFACE !! */ 
printfC Error[2] = %If Nn",Error[2]); 

♦obsres = .4; /* Full diveplane deflection */ 

return Major_Error; /* Depth Sensor error code */ 

) 

if (flag = 0) { 

return No_Error; /* Sensor readings appear normal do not use observer */ 

} 

else { 

diveplane(ex, &divres); 

♦obsres = divres; 
return Sensor_Error; 

} 

} 

Dive plane function determines the appropriate radian angle for the 
dive fins. The arguments are : ex = state error vector, divefin value 

*/ 

void diveplane(double ’•'ex, double *divres) 

I 

static double S[] = | 1.0, 0.7, -0.04 |; 
double nonlin, 
ste, 

stesign, 

diver, 

dive; 
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diver = ’"divres; 

mat_mul(S, 1, 3, ex, 3, 1, (Matrix)&ste); 
nonlin = ste; 
stesign = sign(ste); 

/*—Saturation Switch for Nonlinearity-*/ 

if (fabs(nonlin) > 0.4) { 
nonlin = 0.4 * stesign; 

) 

diver += nonlin; 

dive = sign(diver); /*—Utilize Signum Function—*/ 

if (fabs(diver) > RUD_STOPS) { 
diver = RUD_STOPS * dive; 

1 

*divres = diver; 
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/* 

* AUV.H 


♦ 

♦ 

ikt 

* 

*/ 


Header file for the AUV project. 

Associated Files : ’auvs.c’ ’autplt.c’ ’matmul.c’ ’signum.c’ 
’model.c’ ’modelprm.h’ 


typedef double *Matrix; 

void mat_mul(Matrtx Ml, int rl, int cl, Matrix M2, int r2, int c2. Matrix M3); 
void control8( double *mstate, double *inputs, 

double ordered_depth, double desired_head, double rpm ); 
int model( double *oldstate, double *inputs, double *dt, double *mstate ); 
double sign( double argument ); 
int observer(double q, double theta, double errz, 

double divres, int order_change, double *obsres); 
void diveplane (double *ex, double *divres); 


/* 

* 

* 

* 

* 

*/ 


signum.c 

This is a generic signum function for use in the A.U.V. project. 

The function accepts an argument, detennines the sign of the argument, 
and returns the sign (+1 or -1). 


double sign( double argument ) 

{ 

return (argument > 0.0 ? 1.0 ; -1.0); 

} 
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mat mul.c 


/* 

3k 

* This function multiples two matrices or vectors (or scalar, matrix mult.) 

* and returns the product, typical usages: 

* A=[l 2 3] B=[4; 5; 6] then mat_mul(A,l,3,B,3,l,(Matrix)&scalar_ans) 

* A=[12 3 B=[12 3 

* 4 5 6 4 5 6 then mat_mul(A,3,33,3.34Tiatrix_ans) 

* 7 8 9] 7 8 9] 

* This function is called by autplt.c, the autopilot program for the AUV 

* project. Dave Riling 16 Jan 90 

*! 

#include <stdio.h> 

typedef double *Matrtx; 

/* Row major access macro */ 

#define Xrm(M,row,col,col_len) (*(M +(row * coMen) + col)) 
void mat_mul(Matrix Ml, int rl, int cl. Matrix M2, int r2, int c2. Matrix M3) 

I 

int i,jjc; 
double sum; 

/* Take this out when code is implemented in the auv */ 
if (cl !=r2) { 

fiprintf(stderr,"mat_mul; matrices rows/cols not compatibleNn"); 
exit(l); 

I 

for (i=0; i < rl; i++) 

for (j=0; j < c2; j++) { 
sum = 0.0; 

for(k=0; k < cl; k++) { 

sum += (Xrm(Ml,i,k,cl) * Xrm(M24cJ,c2)); 

) 

Xrm(M3,i,j,rl) = sum; 


119 




APPENDIX C. INTERFACE AND MODEL PROGRAMS 
Included in this appendix are the interface module and the dynamic equations of 
motion model both written in "C" for the AUV. Also included is the header file for the 
model. The code in this appendix is not complier specific, and a copy can be obtained 
by contacting Professor Roberto Cristi, Naval Postgraduate School (see Initial Distribution 
List). 

The model in "C" calls one external function, signum.c, which is located in 
Appendix B. This model updates the 12 states depicted in Table 2. of this thesis using 
the dynamic equations of motion modeled after the U.S. Navy’s Swimmer Delivery 
Vehicle. The interface program calls th nodel after the controller modifies the necessary 
control surface commands (i.e.. Dive plane, mdder, and RPM). 
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/* 

* MODEL.C 

* function state = model(oldstate,inputs,dt) 
*! 


#include <math.h> 

#include <stdio.h> 

#include "modelprm.h" 

double sign(double); 

int model(double *oldstate, double *inputs, double *dt, double *mstate) 

{ 

int j, k; 

double u, V, w, p, q, r, phi, theta, psi; 

double dr, ds, db, rpm, delt; 

double mass, latyaw, norpit, re, termO; 

double signu, signn, eta, cdO, ct, ctl, eps, xprop; 

double ucf[4], fp[6], 

double tmpl, tmp2, tmp3, tmp4; 

double cos_theta, sin_theta, tan_theta; 

double cos_phi, sin_phi, cos_psi, sin_psi; 

u = oldstate[0]; 

V = oldstate[l]; 
w = oldstate[2]; 
p = oldstate[3]; 
q = oldstate[4]; 
r = oldstate[5]; 
phi = oldstate[9]; 
theta = oldstate[10]; 
psi = oldstate[l 1]; 
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dr = inputs [0]; 
ds = inputs[l]; 
db = inputs[2]; 
rpm = inputs[3]; 

delt = *dt; 


latyaw = norpit = 0.0; 
mass = weight/g; 
re = u*l/nu; 

signu = sign(u); 
signn = sign(rpm); 
if (fabs(u) < xltest) 
u = xltest; 
eta = 0.012*rpm/u; 
re = u*l/nu; 

cdO = 0.00385 + 1.296e-17 * (re - 1.2e7)*(re - 1.2e7); 
ctl = 0.008*l*l/a0; 
ct = ctl*eta*fabs(eta); 

eps = -1.04-signn/signu*(sqrt(ct+1.0)-1.0)/(sqrt(ctl+1.0)-1.0); 
xprop = cdO*(eta*fabs(eta) - 1.0); 


/* 

* calculate the drag force, integrate the drag over the vehicle 

* integrate using a 4 term gauss quadrature 
*/ 


for (k=0; k<4; ++k) { 
tmpl = v+g4[k]*r*l; 
tmp2 = w-g4[k]*q*l; 
ucf[k] = sqrt(tmpl’''tmpl + tmp2*tmp2); 
if(1.0e-10 <= ucfik]) { 

termO = ((rho/2.0)*(cdy*hh[kl*tmpl*tmpl 

+ cdz*br[k]*tmp2*tmp2)) *gk4[4]*l/ucf[k]; 
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latyaw += termO*tmpl; 
norpit += term0*tmp2; 

I 

} 

/* 

* force equations 
*! 

I* 

* common sub-expressions 
*/ 

tmpl = (rho/2.0)*l*l; 

tmp2 = tmpl*l; 

tmp3 = tmp2*l; 

tmp4 = tmp3*l; 

cos_theta = cos(theta); 

sin_theta = sin(theta); 

tan_theta = sin_theta/cos_theta; 

cos_phi = cos(phi); 

sin_phi = sin(phi); 

cos_psi = cos(psi); 

sin_psi = sin(psi); 

I* 

* longitudinal force 
*! 

fp[0] = mass*v*r - mass*w*q + mass’''xg*q*q 
■>r mass*xg*r*r - mass*yg*p*q - mass*zg*p*r 
+ tmp3*(xpp*p*p+xqq*q*q + xrr*r*r+xpr*p*r) 

+ tmp2*(xwq*w*q+xvp*v*p+xvr*v*r+u’''q’''(xqds*ds+xqdb*db)+xrdr*u*r*dr) 
+ tmpl *(xvv’''v’''v+xww*w*w + xvdr*u*v*dr+u*w*(xwds’''ds+xwdb*db) 



+ u*u*(xdsds*ds*cis+xdbdb*db*db+xdrdr*dr*dr)) 
- (weight -boy)*sin_theta 
+ tmp2*xqdsn*u*q*ds*eps 

+ tmpl *(xwdsn*u*w*ds+xdsdsn*u*u*ds*ds)*eps 
+ tnipl*u*u*xprop; 


/* 

* lateral force 
*! 

fp[l] = -mass*u*r - mass*xg*p*q + mass*yg*r*r - mass*zg*q*r + 
tmp3*(ypq*p*q + yqr*q*r)+tmp2*(yp*u*p + 
yr*u*r + yvq*v*q + ywp*w*p + ywr*w*r) + tmpl* 

(yv*u*v + yvw*v*w +ydr*u*u*dr) -latyaw +(weight-boy)* 
cos_theta*sin_phi+mass*w*p+mass*yg*p*p; 

I* 

* normal force 
%/ 

fp[2] = mass*u*q - mass*v*p - mass*xg*p*r - mass*yg*q*r + 
mass*zg*p*p + mass*zg’''q*q + tmp3* 

(zpp*p*p+zpr*p*r + zrr*r*r) + tmp2*(zq*u 
*q+zvp*v*p + zvr*v*r) +tmpl*(zw*u*w + zvv*v*v 
+u*u*(zds*ds+zdb*db))-norpit+(weight-boy)*cos_theta*cos_phi 
•ftmp2*zqn*u*q*eps +tmpl*(zwn*u*w +zdsn* 
u*u*ds)*eps; 
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/* 

* roll force 
*! 

fp[3] = -iz*q*r +iy*q*r -ixy*p*r +iyz*q*q -iyz*r*r +ixz*p*q+ 
mass*yg*u*q -mass*yg*v*p -mass*zg*w*j>+tmp4*(kpq* 
p*q + kqr*q*r) +tmp3*(kp*u*p +kr*u*r + kvq*v*q + 
kwp*w*p + kwr*w*r) +tmp2*(kv*u*v + kvw*v*w) + 
(yg*weight - yb*boy)*cos_theta*cos_phi - (zg*weight - 
zb*boy)*cos_theta*sin_phi + tmp3*kpn*u*p*eps+ 
tmp2*u*u*kprop +mass*zg*u*r; 


* pitch force 
*! 

fip[4] = -ix*p*r +iz*p*r +ixy*q*r -iyz*p*q -ixz*p*p +ixz*r*r- 
mass*xg*u*q + mass*xg*v*p + mass*zg*v*r - mass*zg*w*q + 
tmp4*(mpp*p*p +mpr*p*r +mrr*r*r)+tmp3*(mq*u*q + mvp*v*p mvr*v*r) + 
tmp2*(mw*u*w+mvv*v*v+u*u*(mds*ds+mdb*db))+ norpit -(xg*weight- 
xb*boy)*cos_theta*cos_phi+tmp3*mqn*u*q*eps + 
tmp2*(mwn*u*w+mdsn*u*u*ds)*eps- 
(zg*weight-zb*boy)*sin_theta; 


* yaw force 
*/ 

fp[5] = -iy*p*q +ix*p*q +ixy*p*p -ixy*q*q +iyz*p*r -ixz*q*r- 
mass*xg*u*r + mass*xg*w*p - mass*yg*v*r + mass*yg*w*q + 
tmp4*(npq*p’''q + nqr*q*r) +tmp3*(np*u*p+ 
nr*u*r + nvq*v*q +nwp*w*p + nwr*w*r) +tmp2*(nv* 
u*v + nvw*v*w + ndr*u*u*dr) - latyaw + (xg*weight - 
xb*boy)*cos_theta*sin_phi+(yg’''weight)’''sin_theta 
+tmp2*u*u*nprop-yb*boy*sin_theta; 


125 






now compute the f(0-5) functions 


/* 

* 

*/ 


for (j=0; j<6; -H-j) 
for (flj]=0.0Jc=0; k<6; -H-k) 
flj] += xmminv(j][k]*^[k]; 


/* 

* the last six equations come from the kinematic relations 
♦/ 

I* 

* inertial position rates f(6-8) 

V 


f[6] = u*cos_psi*cos_theta + v*(cos_psi*sin_theta* 
sin_phi - sin_psi*cos_phi) + w*(cos_psi*sin_theta* 
cos_phi + sin_psi*sin_phi); 

f[7] = u*sin_psi*cos_theta + v*(sin_psi*sin_theta* 
sin_phi + cos_psi’''cos_phi) + w*(sin_psi*sin_theta* 
cos. _phi - cos_psi*sin_phi); 

f[8] = -u*sin_theta +v*cos_theta*sin_phi +w*cos_theta*cos_phi; 


I* 

* euler angle rates f(9-11) 

*/ 

f[9] = p + q*sin_phi*tan_theta + r*cos_phi*tan_theta; 

fllO] = q*cos_phi - r*sin_phi; 

f[ 11 ] = q*sin_phi/cos_theta + r*cos_phi/cos_theta; 
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first order integration 


/* 

* 

*! 


for (j=0; j<12; j++) 

mstatelj] = oldstate|j] + dilt * f[j]; 

return 0; 

) 
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modelprm.h 


/* 

* 

>K 

* This file contains all of the parameter coefficients 

* used by the file MODEL.C. 

* 

*1 


/* 

* longitudinal hydrodynamic coefficients 
*! 

const double 

xpp = 7.0e-3, xqq = -1.5e-2, xrr = 

xudot =-7.6e-3, xwq = -2.0e-l, xvp = 
xqds = 2.5e-2, xqdb = -2.6e-3, xrdr = 
xww = 1.7e-l, xvdr = 1.7e-3, xwds 

xdsds =-l.Oe-2, xdbdb =-8.0e-3, xdrdr 
xwdsn = 3.5e-3, xdsdsn = -1.6e-3; 


4.0e-3, xpr = 7.5e-4, 

-3.0e-3, xvr = 2.0e-2, 

-l.Oe-3, xvv = 5.3e-2, 

= 4.6e-2, xwdb = 1 .Oe-2, 

= -1.0e-2, xqdsn = 2.0e-3, 


/* 

* lateral hydrodynamic coefficients 
*1 


const double 


ypdot = 

1.2e-4, yrdot = 

1.2e-3, 

yvdot = 

-5.5e-2, yp = 

3.0e-3. 

ywp = 

2.3e-l, ywr = 

-1.9e-2, 

ydr = 

2.7e-2, cdy = 

3.5e-l; 


ypq = 4.0e-3, yqr = -6.5e-3, 

yr = 3.0e-2, yvq = 2.4e-2, 

yv = -l.Oe-1, yvw = 6.8e-2, 
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normal hydrodynamic coefficients 


I* 

* 


*! 


const double 


zqdot 

= -6.8e-3, 

zpp = 

1.3e-4, 

zwdot 

= -2.4e-l, 

zq = 

-1.4e-l, 

zw = 

-3.0e-l, 

zw = 

-6.8e-2, 

zqn = 

-2.9e-3, 

zwn = 

-5.1e-3. 


/* 

* roll hydrodynamic coefficients 
*! 

const double 

kpdot = -l.Oe-3, krdot = -3.4e-5, 
kvdot= 1.3e^, kp= -l.le-2, 
kwp= -1.3e-4, kwr= 1.4e-2, 
kpn = .5.7e-4, kdb = 0.0; 

/* 

* pitch hydrodynamic coefficients 
*! 

const double 

mqdot = -1.7e-2, mpp = 5.3e-5, 

mwdot = -6.8e-3, mq = -6.8e-2, 

mw = l.Oe-1. mvv = -2.6e-2, 

mqn = -1.6e-3, mwn = -2.9e-3, 


zpr = 

6.7e-3, 

zrr = 

-7.4e-3, 

zvp = 

-4.8e-2, 

zvr = 

4.5e-2, 

zds = 

-7.3e-2, 

zdb = 

-2.6e-2, 

zdsn = 

-l.Oe-2, 

cdz = 

1.0: 


kpq= -6.9e-5, kqr = 1.7e-2, 

kr = -8.4e-4, kvq = -5.1e-3, 

kv = 3.1e-3, kvw = -1.9e-l, 


mpr= 5.0e-3, mrr = -2.9e-3, 
mvp = 1.2e-3, mvr = 1.7e-2 
mds = -4.1e-2, mdb = 6.9e-3 

mdsn = -5.2e-3; 
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yaw hydrodynamic coefficients 


* 

*/ 

const double 

npdot =-3.4e-5, nrdot =-3.4e-3, npq = -2.1e-2, nqr = 
nvdot = 1.2e-3, np = -8.4e-4, nr = -1.6e-2, nvq = -l.Oe-2, 

nwp = -1.7e-2, nwr = 7.4e-3, nv = -7.4e-3, nvw = -2.1c-2, 

ndr = -1.3e-2; 


* mass characteristics of the flooded vehicle 
*/ 

const double 

weight = 12000.0, boy = 12000.0, vol ^ 200.0, xg = 0.0, 

yg = 0.0, zg = 0.20, xb = 00, zb = 0.0, 

ix = 1500.0, iy = 10000.0, iz = 10000.0, ixz = -10.0, 

iyz = -10.0, ixy = -10.0, yb = 0.0, 

1= 17.4, rho= 1.94, g= 32.2, nu = 8.47e-4, 

aO = 2.0, kprop = 0.0, nprop = 0.0, xltest = 0.1, 

degrud = 0.0, degstn = 0.0; 

I* 

* define length fractions for gauss quadrature terms 
*/ 

const double 

g4[] = { 0.069431844,0.330009478,0.669990521.0.930568155 1, 
gk40 = i 0.1739274225687, 0.3260725774312, 0.3260725774312, 
0.1739274225687 ); 
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/♦ 

* define the breadth bb and height hh terms for the integration 
*/ 

const double 

br[] = { 75.7/12.0, 75.7/12.0, 75.7/12.0, 55.08/12.0 }, 
hh[] = { 16.38/12.0, 31.85/12.0, 31.85/12.0, 23.76/12.0 }; 


/* 

* assemble inverted mass matrix 
*/ 

const double 

xmminv[6][6] = { 

( 0.2431e-2, 0.2701e-8, 0.1899e-5, 0.1649e-7, .0.5023e-5, 0.3243e-8 ), 

( 0.2679e-8, O.I537e-2, 0.5593e-8, 0.4276e-4, -0.1479e-7, 0.1057e-4 ), 

{ 0.1899e-5, 0.5639e-8, 0.6293e-3, 0.3443e-7, -0.1049e-4, 0.6770e-8 ), 

{ 0.1649e-7, 0.4321e-4, 0.3443e-7, 0.3294e-3, -0.9106e-7, -0.1049e-5 ), 

{ -.5023e-5, -.1491e-7, -.1049e-4, -.9106e-7, 0.2773e-4, -0.1790e-7 }, 

{ 0.3243e-8, 0.1057e-4, 0.6769e-8, -.1052e-5, -0.1790e-7. 0.6561e-4 ) 

); 
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APPENDIX D. FIELD EVALUATION GRAPHICS ROUTINES 

This appeniiiA contains the routines written for field graphical analysis of AUV run 
data. These programs are not intended to produce hardcopy output, but are merely for 
on-the-spot interpretation of AUV run data. They are user friendly and will operate on 
many different graphic systems. 

Particular attention should be paid to the disclaimer in the comment sections of the 
source code. These programs are compiler dependent . They call many nonstandard C 
functions in Libraries specific to MICROSOFT C 5.1 or QUICKC 2.0 . These functions 
are necessary for the graphics routines. This code must be compiled using either of the 
above compilers. 

A copy of this source code is available through Professor Roberto Cristi, Naval 
Postgraduate School (see Initial Distribution List). MICROSOFT C 5.1 and QUICKC 2.0 
are registered trademarks of the Microsoft Corporation. The code contained in the 
libraries called by this source code is available through Microsoft Corporation and its 
affiliated vendors. The U.S. Navy does not support the use of any particular C compiler; 
C compiler used was solely the preference of the author. 
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This program takes a columnized data file (x, y) and plots each column 
with respect to discrete time. It autoscales and labels the graph 
appropriately using the tabular headings created in the "auvs.c" program. 
This file is intended for field use during AUV test. Usage; 

plot <filename> ie. plot auvrun.dat 
This program is conposed of : grph.h setvid.c. 

The setvid.c function sets the program for the correct graphics adapter 
installed (note: please see setvid.c for more information. 

IMPORTANT: The first column of the data file must always be time. This 
column is always the x-axis on the graphs. For a hardcopy output the only 
facility is the use of print screen, since this program was note intended 
for hardcopy output. There is no limitation on the size of the file or the 
number of columns of data it may contain. The larger the file the longer 
the required run time. 

Final note; These files have library functions particular to Microsoft 5.1 
or Quick C 2.0 and as such must be compiled using either of these 
compUers. 

♦/ 


#include "grph.h" 
#include <malloc.h> 


#define BUFFER 256 

float **bigarray; 

stmct text 

{ 

char title[30]. /* structure used for labeling graph */ 

subtitle [30]; 

I label; 
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main( int argc, char **argv ) 

I 

chanenv env; /* setup chart enviroment structure */ 
int ijjc^n.n; 

FILE ♦fptr; 

char headingbuf[BUFFER+l], 

databufIBUFFER+1], 
gridquest[3], 

*label_axis[15]; 
char *tptr; 

if ((fptr = fopen(argv[l];'rt")) = NULL) { 
perror(argv[l]); 
exit(l); 

} 

else I 

fgets (headingbuf,BUFFER,iiptr); /* get the heading */ 

n=0; 

tptr = headingbuf; 

while ((strtok (tptr," ")) != NULL) { 
n++; 

tptr = NULL; 

1 

m=0; 

while (fgets (databuf,BUFFER,fiptr) != NULL) /* get the data */ 
m++; 

rewind(fptr); 

for (k=0; k < n; k++) { 

fscanf(fptr,”%s’',label_axis[k]); /* discard the headings */ 

) 

} 

if ((bigarray = calloc (n,sizeof(float*))) == NULL) ( 

printf (" Something is wrong with memory cannot initialize arrayVi"); 
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exit(l); 


I 

for (i=0; i < n; i++) { 

if ((bigaiTay[i] = calloc(m,sizeof(float))) == NULL) { 

printf (" Check memory cannot load data for array'^"); 
exittl); 

} 

} 

/* Begin graphics set up */ 
for (i=0; i < m; i++) { 

for (j=0; j < n; j++) { 

fscanf (fptr,"%f',&bigarray(j][i]); 

I 


putsC'Enter title of the graph ( limit 30 characters )"); 
gets(label.title); 

putsC'Enter the subtitle of the graph ( limit 30 characters )"); 
gets(label.subtitle); 

/* Begin graphics hardware interface */ 
graphics_mode(); /* call the setvid.c file */ 

_clearscreen( _GCLEARSCREEN ); 

_pg_initchan(); 

_pg_defaultchart( &env, _PG_SCATTERCHART, _PG_POINTANDLINE ); 
strcpy( env.maintitle.title, label.title ); 

env.maintitle.titlecolor =1; /* color 1 = white,text color */ 

env.maintitle.justify = _PG_CENTER; 
strcpy( env.subtitle.title, label.subtitle), 

env.subtitle.titlecolor =1; /* color 1 = white,text color */ 

env.subtitle.justify = _PG_CENTER; 
strcpy( env.xaxis.axistitle.title, "Time"); 
env.chartwindow.border = TRUE; 
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_pg_getpalette( pal ); /* Set chart point value to = blank */ 

pal [1].plot char = 32; 

_pg_setpalette( pal); 
jmpbak: 

putsC'Do you want grids on your graph (y or n)?"); 
gets (gridquest); 
switch (*gridquest) { 
case ’y’: 

env.yaxis.grid = TRUE; 
env.xaxis.grid = TRUE; 
env.yaxis.gridstyle = 4; 
env.xaxis.gridstyle = 5; 

break; I 

1 

case ’n’: 

env.yaxis.grid = FALSE; 
env.xaxis.grid = FALSE; 
break; 
default: 

printfC'Try again. Input lowercase y or n only! \n"); 
goto jmpbak; 

} 

/* Begin plotting values */ 
for (i=l; i < n; i++) { 

strcpy( env.yaxis.axistitle.title, label_axis[i]); 

_Pg_chartscatter( &env, bigaiTay[0], bigarray[i], m); 
getchO; 

1 

_setvideomode( _DEFAULTMODE ); ,/♦ reset the video back to original */ 

/* setting prior to running ploi.exe*/ 

for (i=0; i < n; i++) 
free(bigarray[i]); 
free(bigarray); 
fclose(fptr); 
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setvid.c 


/* 


This function sets the video graphics mode for the graphing routine: 
plot. It checks for the graphics-adapter card that is present in the 
respective computer and sets it automatically to its highest resolution. 
This function is called by "plot.c". It returns a void and accepts no 
arguments.Graphics cards that are accepted by this program are: 

1)VGA 2) EGA 3) CGA 4) HERCULES. 

DISCLAIMER: Due to the configuration of the standard Microsoft ’C’ 
libraries used, this function will always set the graphics card present 
to its highest resolution available regardless of the monitor in use. 
Therefore, if you have a VGA graphics card installed and are using a 
EGA monitor the "plot.exe" program will not work. 

*/ 

#include "grph.h" 

void graphics_mode( void ) 

I 

_getvideoconftg( &myscreen ); 
switch( myscreen.adapter ) { 
case _CGA: 
case _OCGA: 

_setvideomode( _HRESBW ); 
break; 
case _EGA: 
case _OEGA: 

_setvideomode( _ERESCOLOR ); 
break; 
case _VGA: 
case _OVGA: 
case _MCGA: 

_setvideomode( _VRES16COLOR ); 
break; 
case _HGC: 

_setvideomode( _HERCMONO ); 
break; 
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default: 

printf( "This program requires a graphics card."); 
exit(O); 

) 

_getvideoconfig( &myscreen ); 

maxx = myscrcen.numxpixels - 1; /* this information is not used *! 

maxy = myscreen.numypixels - 1; /* currently, it is used for */ 

} /* manual axis scaling */ 


This is the header file for the program "graph.exe". 
Associated files are ; ’plot.c’ and ’setvid.c’. 

*! 

#include <stdlib.h> 

#include <stdio.h> 

#include <conio.h> 

#include <graph.h> 

#include <pgchart.h> 

#include <string.h> 

#include <math.h> 

typedef enum {FALSE. TRUE) boolean; 
void graphics_mode(, void ); 
stmct videoconfig myscreen; 
int maxx, maxy; 
palettetype pal; 








APPENDIX E. HARDWARE DESIGN SCHEMATICS 


The diagrams in this appendix pertain to the designs discussed in Chapter FV. this 

/ ^ 

thesis. These diagrams are of the active-RC filter circuit and the Synchro-to-Resolver 
circuit. For explanation and positioning of these curcuits, please refer to Chapter IV. 
These circuits are intended for use in the signal conditioning network of the AUV. For 
further assistance refer to the technical notes listed in the references. These notes are 


available in the AUV library. 
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